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ABSTRACT 
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This  report  develops  10  different  approaches  to  the  target  tracking 

problem  based  on  bearing  only  observations.  Its  purpose  is  to  form  the 

basis  for  an  extensive  simulation  study,  aimed  at  achieving  the  best 

possible  tracking  performance  for  this  tracking  problem.  Included  in 

the  report  are  also  a  proposed  initialization  routine  for  bearing 

only  trackers,  and  a  maneuver  detection  algorithm  with  a  proposed  action 

scheme  following  the  maneuver  detection. 
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SUMMARY  AND  CONCLUSIONS 


Ten  different  approximative  nonlinear  filtering  approaches  to  the 
target  tracking  prol'lem  based  on  bearing  only  measurements  have  been 
developed. 

These  approaches  include  two  Extended  Kalman  filters  (Cartesian  and 
Polar  coordinate  system  representation) ,  two  second  order  Gaussian  rilters 
(Cartesian  and  Polar),  fiva  different  iterated  Extended  Kalman  filters, 
and  one  approach  consisting  of  M  parallel.  Extended  (Cartesian)  Kalman 
filters . 

The  polar  coordinate  system  representation  of  the  target  motion 
provided  the  necessary  insight  and  equations  for  us  to  attack  the 
initialization  problem  based  on  bearing  only  information.  Based  on  the 
two  assumptions;  1)  Straight  target  trajectory  and  2)  Noisefree  bearings, 
we  could  show  that  given  a  target  range,  the  target  velocity  components 
could  be  calculated  from  three  consecutive  bearings,  with  no  constraints 
on  the  observer's  trajectory  over  the  observation  interval.  We  could  also 
show,  that  if  the  observer's  velocity  was  zero,  we  could  calculate  the 
target's  heading  exact,  even  if  the  assiamed  range  was  false. 

By  including  a  fourth  bearing,  and  introducing  the  constraint  that 
the  observer  should  be  maneuvering  during  the  observation  interval,  we 
also  derived  the  equation  for  calculation  of  range  to  target. 

In  order  to  take  into  account  the  fact  that  the  observations  are  noisy, 
a  procedure  for  smoothing  of  initial  range  and  velocity  data  calculated 
from  a  batch  of  bearing  observations  was  proposed. 

In  order  to  adapt  the  straight  trajectory  filters  to  the  curved 
trajectory  case,  which  results  from  a  maneuvering  target,  we  derived  the 


IV 


structure  for  a  likelihood  ratio  test  based  on  the  principle  of  comparing 
the  innovation  sequence  with  its  expected  statistics,  based  on  the  "no 
maneuvre"  hypothesis.  When  the  actual  variance  of  the  innovation  sequence 
becomes  consistently  larger  than  its  expected  variance  over  the  most 
recent  L  samples,  the  "target  maneuvre"  hypothesis  is  accepted. 

The  proposed  action  scheme  following  the  maneuvre  detection  included 
two  main  features: 

1.  A  reprocessing  of  the  obsem/ations  ontained  in  the  time 
period  At  prior  to  the  maneuver  detection,  where  At  is 
the  nominal  time  delay  between  the  start  of  the  maneuvre 
and  its  detection. 

2.  Imposing  lluiiced  memory  on  the  filter,  by  increasing  the 
velocity  elements  of  the  covariance  matrix,  before  the 
reprocessing  of  observations  described  above  takes  place. 

The  result  of  this  scheme  will  be  a  better  utilization  of  the  obser¬ 
vations  obtained  during  the  target  maneuvre.  When  the  reprocessing  of 
the  observations  obtained  during  the  time  period  At  are  finished,  a  dis¬ 
crete  jump  to  a  more  correct  position  and  velocity  of  the  state  vector 
at  the  time  when  the  maneuvre  was  detected,  will  be  the  result. 

The  purpose  of  this  report  is  to  be  a  mathematical  basis  for  an 
extensive  simu3ation  study  where  the  performance  of  the  different  filtering 
approaches  to  bearing  only  tracking  can  be  compared. 

When  this  simulation  study  has  been  performed,  we  will  be  in  the 
position  to  give  an  answer  to  the  question:  To  what  extent  is  it  pos¬ 
sible  to  overcome  the  three  main  problems  with  bearing  on]y  tracking: 

1)  The  poor  degree  of  observability,  2)  The  nonlinearity  of  the  problem. 
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claiming  for  the  best  possible  linearization  trajectory,  and  3)  The 
target  maneuvre  detection  and  handling  problem. 

We  have  not  made  any  efforr  to  analyze  and  compare  the  con5)utational 
complexity  nor  the  memory  capacity  requirements  of  the  different  filters. 
The  reason  for  this  is  that  we  didn't  want  these  aspects  of  the  problem 
to  constraint  ovir  choice  of  filtering  approaches  at  this  stage. 
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1.  INTRODUCTION 
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1.1  Problem  Statement 

Our  objective  is  to  investigate  the  target  tracking  problem  based 
on  bearing  observations  from  a  single,  moving  observer. 

Ovir  approach  will  be  threefold: 

1.  D:^rive  different  mathematical  approaches  to  the  filtering 
problem  for  targets  moving  along  straight  trajectories, 

2.  Derive  more  optimal  initialization  routines  based  only  on 
bearing  obseirvations  from  the  moving  observer. 

3.  Derive  maneuvre  detection  algorithms,  which,  together 
with  specific  action  patterns  following  the  maneuvre 
detection,  allows  the  straight  trajectory  filters  to 
adapt  tc  curved  trajectories. 

We  restrict  the  problem  to  tracking  in  the  x-y-plane. 

The  observer's  position,  velocity  and  acceleration  are  known 
functions  of  time,  with  known  accuracy  (lo  values). 

The  bearing  sensor's  accuracy  is  known  (la). 


1.2  Report  Outline 

The  composition  of  this  report  can  be  described  as  follows: 

Chapter  2  introduces  the  difficulties  that  exists  for  this  type  of 
tracking,  and  gives  an  outline  of  the  different  approaches  to  straight 
trajectory  tracking  that  will  be  developed. 

In  Chapter  3  we  develop  the  mathematical  equations  for  10  different 
filtering  approaches  to  this  tracking  problem. 

In  Chapter  4  the  initialization  problem  is  analyzed,  and  necessary 
equations  for  an  initialization  routine  for  bearing  only  trackers  are 
proposed. 


m  m 


Chapter  5  gives  a  short  resume  over  earlier  approaches  to  Buuieuvering 
target  tracking,  and  continues  with  the  detailed  equations  for  a  proposed 
maneuvre  detection  and  handling  scheme  for  the  different  filtering 
approaches  given  in  Chapter  3. 

The  piirpose  of  this  report  is  to  form  the  basis  for  a  simulation 
study,  where  the  different  tracking  approaches  are  compared,  and  the  fil¬ 
tering  approach  wi  .h  the  best  overall  performance  can  be  selected. 

Chapter  6  gives  a  few  guidelines  to  be  taken  into  account  while  planning 
and  performing  this  simulation  study. 


^  I 
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2.  PROBLEM  ANALYSIS  AND  APPROACH  PROPOSALS 

The  main  problem  with  this  type  of  tracking  is  the  poor  degree  of 
observability.  It  is  well  known  that  the  success  of  this  tracking 
scheme  depends  entirely  on  the  observer's  maneuvering.  Simultaneously, 
in  most  cases,  the  tracker  has  to  be  based  on  a  maneuvering  scheme 
selected  on  other  criterias  than  tracking  performance.  This  is  due  to 
reasons  Irke; 

-  Narrow  waters 

-  Not  reveal  the  observer's  position 

-  Not  restrict  the  captain's  decision  space. 

The  topic  of  selecting  the  optimal  maneuvre  in  order  to  maximize 
the  observability  has,  however,  been  treated  by  D.J.  Murphy  [11* 

Another  problem  is  the  necessity  to  perform  linearization  about 
the  target  trajectory.  However,  this  trajectory  is  unknown.  The  pur¬ 
pose  of  our  tracker  is  to  estimate  this  trajectory.  Now,  if  lineari¬ 
zation  is  performed  about  the  a  priori  estimate  of  the  state  vector, 
which  is  the  most  obvious  thing  to  do,  the  utilization  of  observations, 
when  the  estimated  trajectory  is  far  from  the  correct  one,  will  be 
poor.  This  is  the  case  in  the  initialization  phase,  and  after  a 
target  maneuvre. 

A  proposed  initialization  routine  for  bearing  only  trackers  will 
be  developed  in  Chapter  4. 

The  third  main  problem  with  this  type  of  tracking  is  the  maneuvre 
detection  and  handling  problem.  If  the  target  performs  a  meineuvre 
after  a  stable  target  solution  is  established,  it  is  possible  to  detect 
the  occurrence  of  the  maneuvre  (after  a  certain  period  of  time)  without 
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the  observer  mcuieixvering.  However,  in  order  to  arrive  at  the  new, 
post-maneuvre  target  course  and  speed,  an  observer  nicineuvre  normally 
has  to  take  place.  This  is  due  to  the  time  delay  between  the  maneuvre 
occurrence  and  its  detection,  resulting  in  range  to  target  error  at 
the  target  maneuvre  detection  time  point.  Since  this  time  delay  will 
vary,  depending  on  the  type  and  size  of  the  target  maneuvre,  the  range/ 
velocity  ambiguj.  v  ceinnot  be  resolved  exactly  without  an  observer 
maneuvre.  Maneuvre  detection  and  following  actions  patterns  will  be 
proposed  in  Chapter  5. 

The  question  is  now:  How  can  these  three  problems  be  overccxne? 

Or  to  put  it  more  correctly:  To  what  extent  do  different  approaches 
to  this  target  tracking  scheme  overcome  these  three  problems? 

Existing  literature  on  the  siabject  fail  to  give  an  answer  to  this 
question. 

Our  intention  is,  therefore,  to  develop  a  number  of  different 
approaches  to  this  target  tracking  problem,  which  can  form  the  basis 
for  an  extensive  simulation  study,  where  the  different  approaches  are 
compared.  When  this  simulation  study  is  finished,  our  basis  for  giving 
a  reliable  answer  to  the  question  above  should  be  greatly  is^roved. 

The  following  10  approaches  are  proposed: 

1.  Extended  Kalmein  filter,  Cartesian  Coordinate  system  repre¬ 
sentation.  State  vector: 

Tx  1 


y 


(2.1) 
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observation  equation: 

+  w  (2.2) 

where 

X  =  target's  x  -  coordinate 
y  =  target's  y  -  coordinate 
=  target's  x  -  velocity 

ss  tcirget's  y  -  velocity 

X  =  observer's  x  -  coordinate 
s 

y  =  observer's  Y  -  coordinate 
■'s 

4)  =  bearing  from  observer  to  target  rel.  north, 
w  =  observation  noise 

2.  Extended  Kalman  filter,  polar  coordinate  system  representation. 
State  vector; 


Observation  equation: 


(j)  =  tl  0  0  0]^  +  w 


(2.4) 
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where  (j),  v^. 


V  and  w 

y 


are  defined  earlier,  and 


R  =  range  from  observer  to  target 


3.  Second  order  Gaussian  filter,  representation  as 

4.  Second  order  Gaussian  filter,  representation  as 

5.  Iterated  Extended  Kalman  filter,  representation 
iteration  scherau  ;  in  Jazwinski  [2] ,  pp.  278-279. 


in  1. 
in  2. 
as  in 


1, 


6.  Iterated  Linear  Filter-Smoother.  Representation  as  in  2, 
iteration  scheme  as  in  Jazwinski  [2] ,  pp.  279-281. 


7.  Global  Iterated  Filter,  Representation  as  in  1,  iteration 
scheme  as  proposed  by  Jazwinski  [2] ,  pp.  281. 

8.  Global  Iterated  Filter.  Representation  as  in  2,  iteration 
scheme  as  in  7. 


9.  Extended  Kalman  filter,  Cartesian  coordinate  system  repre¬ 
sentation.  State  vector; 


LVJ 


(2.5) 


where 


N 

c 


=  May  be  variable  number  >  1 

=  State  vector  at  sample  k,  representation  as  in  1. 


=  State  vector  at  san^le  k-N,  representation  as  in  1. 


Observation  equation: 


(2.6) 


This  scheme  will  process  each  observation  twice.  Details  outlined  in 
Chapter  3.6.3  ("Serial"  filter  approach) 


10.  Parallel  filter  approach,M  filters,  each  with  Cairtesian  coordinate 
system  representation  as  in  approach  1,  are  initialized  with  range 


+  i  •  Ar,  i  =  1,2,...,M  (2.7) 

where  R^  and  AR  are  constants.  The  a  priori  variances  on  R^  for  each 
filter  are  assumed  low,  in  order  to  stablize  range  for  each  filter. 

The  resulting  state  vector  for  this  filtering  scheme  is  given  by: 


^P(R.=R/Zj^)4. 


(2.8) 


where  the  probability  p(R^  =  R/Zj^)  has  to  be  determined 
Chapter  3.7. 


Details  in 


3.1  Extended  Kalman  filter,  Cartesian  Coordinate  System. 

The  Cartesian  Coordinate  system  representation  is  the  most  connon 
way  to  model  the  target  motion  dynamics.  Several  papers  on  tcurget 
tracking  use  this  representation  [3] -[11],  and  most  probably  in  the 
majority  of  the  existing  teirget  tracking  systems  of  this  kind  in  operating 
order,  this  representation  is  used. 

Assuming  the  target  is  nonmanetivering,  the  following  mathematical 
model  can  be  established: 


The  geometrical  situation  is  depicted  in  Fig.  3.1; 
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X 


X 


s 


Pig.  3.7.  Target-Observer-Geometry. 

The  different  variables  in  equations  (3.1)  and  (3.2)  should  be 

T 

self -explaining  from  Fig.  3.1,  except  for  w  and  v  =  [v^  V2]  .  These 
are  the  measurement  and  the  process- noise,  respectively. 

Since  the  tracker  is  going  to  be  realized  on  a  digital  con^uter,  the 
discrete  version  of  the  equations  (3.1)  and  (3.2)  are  sought.  We  get 


(1/T  -  sample  frequency)  [3] ; 

=  <}>(T)5^  +  9(T)v 

(3.3) 

=  91^)  +  \ 

(3.4) 

Here,  <{)(T),  6(T)  and  g(3^)  defined  by: 
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V  0 

Ik 


0  V, 


(3.9) 


Further,  we  define; 


=  -iSL 


-k,k-l  L  ^ 


k  k 


0  0 


(3.10) 


where  ^  ^  ^  is  the  a  priori  estimate  of  the  state  vector  at  time  k. 

The  Extended  Kalman  filter  equations  for  the  system  described  by 
equations  (3.3)  to  (3.10)  are  the  following: 


Initialization ; 


^,-1  “  -0 


(3.11) 


p  s  t> 

^0,-1  *^0 


(3.12) 


Observation  integration: 


%,k  "  ^,k-l  \^^k  "  ^,k-l^ 


(3.13) 


S:,k-1  "  ^^^,k-l^ 


(3.14) 


(3.15) 


^,k  =  ^  Vk^ 


(3.16) 


Time  testing: 


%M,k  =  ♦«>4,k 


‘’k+l,k  °  +  6(t)V|^6(t)'' 


(3.17) 


(3.18) 


IMV'iWiKIM'MimH.uiHH 


A  discrete  Polar  coordinate  system  representation  of  the  non- 
nanevivering  target  can  be  derived  mathematically,  however,  the  easiest 
way  is  to  use  a  geometrical  approach.  Fig.  3.2  shows  the  relative 
gecanetrical  situation,  as  seen  from  the  observer  (the  coordinate 
system  is  fixed  to  the  observer) : 


By  refering  to  Fig.  3.2,  the  following  two  equations  can  be  estab¬ 
lished  directly: 


VVpk/ 


+  V, 


Ik 


(3.19) 


«k+l='/V^pk>  + 


2k 


(3.20) 
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where  and  are  additional  white  Gaussian  noise  processes. 

The  variables  relative  displacement  between 

observer  and  target  over  the  sample  period  [kT,  (k+l)T] ,  along  and 
across  the  line  of  sight  to  teirget,  These  variables  can  be  given 

by; 


• 

/ 

r  1 

•  «■ 

'Sk 

‘^sk 

-V 

‘1 

'^''Tk 

where  the  treuis formation  matrix,  s  is  given  by; 


(3.21) 


cos  (t>j^ 

-sin 

sin  (|>j^ 

cos 

(3.22) 


T  T 

and  ^^^sk  ^^sk^  target's  and  the  observer's 

absolute  displacement  in  x,y  -  direction  during  the  sample  period 
[kT,  (k+l)T]. 

Since  the  assmption  is  made  that  the  target  is  not  maneuvering,,  we 
have; 


^xk 

=  T  . 

'  t> 

1 _ 

(3.23) 


where 


V  =  target  absolute  velocity  in 


Vyj^  =  target  absolute  velocity  in 


x-direction  at 
y-direction  at 


time  kT 
time  kT 
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The  observer's  displacement  dxiring  the  san5>le  period  [kT,  (k+l)T] , 

T 

[AXgj^Ay^j^]  ,  is  given  by  the  observer's  dead  reckoning  system.  Assuming, 
that  the  observer's  acceleration  is  constant  over  the  san^le  period,  we 
can  write; 


^sxk 

.  T  +  i 

®sxk 

2 

_^syk_ 

where 


^sxk  ~  observer's  velocity  in  the  x-direction  at  time  kT 
v^yj^  =  observer's  velocity  in  the  y-direction  at  time  kT 


a  ,  «  observer's  acceleration  in  the  x-direction  at  tine  kT 
syk 

a^yj^  =  observer's  acceleration  in  the  y-direction  at  tine  kT 


Now,  selecting  the  Cartesian  velocity  crai^Ksnents  v^  and  v^j^  as 
representation  for  the  target  velocity,  the  total,  nonlineeu:  Polar 
coordinate  system  representation  of  the  target-observer  relationship, 
is  given  by; 


“^k+l 

^  (w) 

^Ik 

^k-'-l 

+ 

^2k 

^xk+1 

^xk 

.""y  k+1 

/4k- 

(3.25) 
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equation  (3.25)  can  be  written: 

Fiirther,  defining  the  observation  matrix; 
H  =  [1  0  0  0] 


(3.26) 


(3.27) 


(3.28) 


(3.29) 


the  observation  equation  for  the  system  described  by  equation  (3.28) 
will  be: 

\  =  H  •  2^  +  \  (3.30) 

Equations  (3.28)  and  (3.30)  cire  our  final  equations  for  the  Polar 
coordinate  system  version  of  the  target  tracking  p.roblem.  As  we  can 
see,  the  system  dynamics  are  nonlinear,  while  the  observation  equation 


is  lineeir. 
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In  order  to  utilize  the  Extended  Kalman  filter  on  this  system,  we 
have  to  develop: 


=  ^,1 


The  developnent  of  Fj^  is  done  in  APPENDIX  A.  The  result  is: 


where 


■Sc'^k  *  “•pk> 

T.AL  , 
yk 

*'“-*k 

4*1 

41 

4*1 

\-'%t 

VV 

t.al^ 

Vi 

Vi 

Vi 

Vi 

0 

0 

1 

0 

1 

0 

0 

0 

1 

Al  . 
xk 

=  fk  '%c 

•  cos  (|)j^ 

Al  . 
yk 

=  ®k  *  ''^pk>  “=  ♦k  -  'Ne 

sin  <J>j^ 

or,  by  inserting  for  ^pj^.  frcxn  equation  (3.21) 


“■vk  '  \  ♦k  *  ''''•He  -  "S': 


(3.31) 


(3.32) 


(3.33) 


(3.34) 


sk 


(3.35) 

(3.36) 


The  Extended  Kalman  filter-equations  for  the  system  described  by 
equations  (3.28)  amd  (3.30)  are  the  following: 
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Initialization: 


II 

rH 

1 

(3.37) 

pP  =  pP 

0,-1  0 

(3.38) 

Obseirvation  Integration: 

S,k  S,k-1  *St^\“^,k-l^ 

(3.39) 

^k,k-i  *^,k-l 

(3.40) 

(3.41) 

(3.42) 

Time  updating: 

4i,k  ■  ,k> 

(3.43) 

II 

+ 

(3.44) 

where  the  process  noise  covaricince  matrix,  V^,  is  given  by 


4 


0 

0 


0  yP 

3k 


0 

0 

0 


0 

4k 


(3.45) 


In  order  to  get  identical  initial  conditions  for  the  two  repre¬ 


sentations  given  in  Section  3.1  and  3.2,  the  following  relationship 
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exists  between  the  initial  conditions  given  in  equations  (3.11),  and 
(3.37),  (3.38): 


sin  (()q  0 

cos  (|>Q  0 

0  1 

0  0 


Further,  defining: 


T  = 


“ 

R  cos  <j)Q 
-R  sin  ((Iq 


0 

0 


sin  (fi^  0  0 

cos  (j)^  0  0 

0  10 

0  0  1 


we  have: 


(3.46) 


(3.47) 


(3.48) 


3.3  Second  Order  Gaussian  Filters. 

The  following  development  follows  the  spirit  of  Jazwinski  [2] , 
pp.  91,  336-346,  362-365,  and  Gelb  [12],  pp.  191-192.  We  are,  however, 
interested  in  a  system  representation  where  both  system  dynamics  and 
observation  are  discrete,  while  [21  and  [12]  are  concerned  with  continuous 
system  representation,  and  discrete  observations.  Therefore,  the  discrete 
version  of  the  second  order  time  updating  equations  have  to  be  developed. 

Jazwinski  [2]  defines  4  different  possible  solutions  to  second  order 


filtering; 
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1.  The  tnincated  second  order  filter. 

2.  The  Gaussian  second  order  filter. 

3.  The  modified  tnincated  second  order  filter. 

4.  The  modified  Gaussian  second  order  filter. 


approach  no.  4,  which  also  has  been  developed  by  Wishner 
For  this  approach  the  following  assumptions  are  made 

is  symmetric  and  "close  to  the  mean". 

2.  The  third  central  moment  is  zero. 

3.  Assuming  Gaussian  density,  the  fourth  central  moment  is 
approximated  by: 

B  <3-«) 

4.  The  fifth  and  higher  order  moments  are  neglected. 


We  select 
et  al.  [13]. 
about  p  (Xj^) ; 

1.  p(v 


In  order  to  proceed  the  development,  a  couple  of  operators  have  to 
be  defined,  namely  1)  9^{£(x)/B)  and  2)  (9^^  P^9^f^) : 

2 

1)  The  operator  9^(£{x),B),  for  any  function  ^(x) ,  any  x  and 
any  matrix  B  is  a  vector  whose  i^^  element  is  defined  by: 


9^ . (f ,B)  =  trace 
xi  — 


9x 


9f. 
— 1 


9x 


B 


(3.50) 


2  2  2 

2)  The  operator  [9jjf(x)P  function  f(x),  any  x  and 


any  symmetric  matrix  P,  is  a  symmetric  matrix  with  elements  (i,  j)  [2] : 
n 


9^f. 

1 


9^f. 


P„  P, 


,  ZmJ  9x,  9xn  to  kq  9x  9x 
k,  ,p,q  He  £  ^  ^  p  ( 


(3.51) 
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Wishner  et  al  [13]  gives  another  definition  of  this  operator,  which 

th 

tvirn  out  to  give  identical  result.  The  [i,j]  element  is  in  [13]  given 
by: 

i 4  4  4  4  “3'-” 

In  the  following,  we  will  use  the  definition  in  eq>  (3.52)  for  this 
operator. 

With  the  definition  of  these  two  operators  in  mind,  we  are  ready  to 
develop  the  moclified  Gaussian  second  order  filter. 

The  system  and  observation  equations  are  etssiBoed  to  have  the  following 
form: 
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We  recall  the  definitions  of  equations  (3.10)  and  (3.31).  Now,  taking 
the  expectations  of  equations  (3.55)  and  (3.56) ,  making  use  of  the 
assumptions  that: 


(3.57) 

-  a 


we  get: 


4+i,k  '  -  it4,k>  I  ‘'k,k> 

^,k-l  '  8j^_^_^(9.fk,k-l> 


(3.58) 


(3.59) 


Equation  (3.58)  is  the  second  order  approximation  difference  equation 
for  2^  between  observations.  We  now  seek  a  recursive  equation  for  the 
covariance  matrix  P  between  observations.  Using  (3.53),  (3.55),  (3.31) 

iC 

and  (3.58)  we  get: 


^k+l,k  ®^^^+l"^+l,k^  ^^+l"^+l,k^  ^ 


E{[Fj^.A^^k  2  ^Xj^  ^^,k  *  ^,k^  2  ^Xj^  j^^-'^k,k^  ^ 


=  FP  F  +V  +  L. 
k  k,k  k  k  ^ 


(3.60) 


where  L  is  given  by: 
k 


^  ®‘'4,k ''4,k^,k'''> 

1  ' 

^,k 


(3.61) 
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By  use  of  the  approximation  given  in  equation  (3.49) ,  can  be  reduced 
to  [2] : 


\  =  I  ^4  1 

K,k 


.  )pr .  3: 

-^,k  k,k  Xj^ 


)) 


(3.62) 


Equations  (3.60)  and  (3.62)  are  the  sought  recursion  for  Pj^  between 
observations. 

The  equations  for  the  modified  Gaussian  second  order  filter  at  an 
observation  is  given  in  [2]  : 


»k 


(3.63) 


'■k.k  -  «-Sc\>\,k-l"-‘=k\>''  *  "Sc^k^ 


(3.64) 


\  ~  \,k-A^W,k-l\  \  V 


(3.65) 


(3.66) 


The  necessary  equations  for  the  modified  Gaussian  second  order  filter 
are  now  established,  and  constitutes  of  et^uations  (3.58),  (3.60)  and 
(3.62)  for  time  updating,  and  (3.63),  (3,59),  (3.64)  -  (3.66)  for  ob¬ 
servation  integration. 

The  equations  will  nov;  be  specialized  to  the  two  different  representa¬ 
tions  of  the  target  tracking  problem. 


3.3.1  Cartesian  Coordinate  System  Model 

The  Cartesian  system  model  representation  of  the  target  motion  with 
bearing  only  measurements  is  given  by  equations  (3.3)  to  (3.7).  Since 
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the  system  dynamics  are  linear,  the  second  order  filter  equations  for  time 
updating,  equations  (3.58) ,  (3.60)  and  (3.62)  reduces  to  the  normal  Kalman 
filter  equations,  given  by  equations  (3.17)  and  (3.18).  The  observation 
equation  (3.4),  however,  is  nonlinear,  and  equations  (3.59)  and  (3.66) 
have  to  be  evaluated  for  the  nonlinear  function  9(^)  given  by  equation 
(3.7). 

From  equations  (3.59)  and  (3.7)  we  get: 


ft  _  ^  — -1/  k.k-1  sk  \ 


+  b. 


(3.67) 


where  bj^  is  given  by  (see  equations  (3.59)  and  (3,50)): 


(3.68) 


1  ,k2  ..  -1,  k  ..  .  1  .  I  9  r  3  tan‘^(*)l'^i  I 

“  2  “  2  [-Jg - j  I  '  >,k-l| 

-  “%,k-l 


Defining  (see  Fig.  3.1) 


Ax  = 


\,k-l  %k 


'  ''k,k-l  ■  ''sk 


(3.69) 


we  can  calculate 


9x 


^  .  -1  ,Ax. 

3  tan  ^ 


3x 
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The  result; 


f  2Ax-Ay 


_3_ 

-it¬ 
'd  tan  ■‘‘xA 

ii 

3x 

A  2  .  2 

Ax  -Ay 


0 

0 


Ax  -Ay 


2Ax.Ay 

0 

0 


0 

0 


0 

0 


(3.70) 


Based  on  equation  (3.70) ,  equation  (3.68)  becomes 


^  2  R 


trace 


-2AxAy  •  +  (Ax^-Ay^)?^^#  -2AxAyP^2  (Ax^-Ay^)P22  0  0 

(Ax^-Ay^)P^j^  +  2AxAyP2j^/  (Ax^-Ay^)F^2  +  °  ^ 


0 

0 


0 

0 


Finally; 


TJ  V)  (Pji  +  Pij) 


2  R 


0  0 

0  0 
(3.71) 

<3.72) 


Next  we  have  to  evaluate  equation  (3.66)  based  on  equations  (3.52)  and 
(3.7).  The  result  is 


\  =  [2A^^Ay^p2^+P2^-pJ-?2j)  -  2AxAy  (Ax^  V)(Pii-P22)  <Pi2-'P2l^ 

+  (Ax  -Ay  )  (P2^iP22  ^12*^21^^ 


(3.73) 


In  equation  (3.71)  -  (3.73)  the  variables  P^^,  P^2»  P2i»  ^22 


the  position  elements  of  the  covariance  matrix  P  ,  i . e . , 

Jv  f  IC"*X 
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ii 

^12 

^13 

^4 

21 

^22 

^23 

^24 

31 

^32 

^^33 

^4 

» 

41 

^42 

^43 

^44 

(3.74) 


The  modified  Gauosian  second  order  filter  for  the  Cartesian  system 
model  representation  of  the  target  motion  can  then  be  summarized  as  follows: 


Initialization :  Equations  (3.11)  and  (3.12). 

Observation  Integration ;  Equations  (3.13),  (3.67),  (3.72),  (3.65),  (3.73) 
and  (3.16). 

Time  updating;  Equations  (3.17)  and  (3.18). 


3.3.2  Polar  Coordinate  System  Model 

The  polar  coordinate  system  representation  of  our  cracking  problem  is 
given  by  equations  (3.28)  and  (3.30).  In  this  case  the  system  dynamics 
are  nonlinear,  while  the  observation  equation  is  linear. 

The  second  order  filter  equations  for  time  updating,  equations  (3.58), 
(3.60)  and  (3.62)  have  then  to  be  specialized  to  our  polar  coordinate  system 
model,  while  the  observation  equations,  equations  (3.63) - (3.66) ,  (3.59), 
reduces  to  the  normal  Extended  Kalman-filter  equations,  given  by  equations 
(3.39)-{3.42). 


From  equation  (3.58)  we  get: 


/vP  -  .  f  3  * 

\+l,k  2  ^ 


(3.75) 
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where  the  vector  Cj^  has  elements  i  given  by: 


C,  .  =  trace  ,  ^ 
ki  \  dX 


{3.76) 


Further,  we  have  to  calculate  the  matrix  Lj^  given  by  equation  (3.62). 

The  calculation  of  ^  and  are  performed  in  APPENDIX  B.  The  results 

are: 


I  fl..P.. 
31  13 


'^Ik 

‘^2k 

.^4k. 

E  f2..P.. 
31  13 


0 

0 


(3.77) 


4  4 

Z  S..S..  Z  s.,t.. 


j,i=l 


•  .  1  31  i3 
3»i=l  ^  ^ 


Z  t..s..  Z  t..t.. 

.  .  ,  T1  11  .  .  ,  11  11 

3,1=1  ^  3»i=l 


0 

L  0 


0 

0 


0 

0 


(3.78) 


The  elements  of  the  matrices  FI,  F2,  S  and  T  are  given  in  APPENDIX  B. 

The  modified  Gaussian  second  order  filter  for  the  Polar  Coordinate 
system  model  of  the  tracking  problem  can  then  be  summarized  as  follows: 


Initialization;  Equations  (3.37)  and  (3.38) 


Observation  integration;  Equations  (3. 39) -(3. 42 ) . 

Time  updating;  Equations  (3.75),  (3.77),  (3.60)  and  (3,78). 

3.4  Iterated  Extended  Kalman  Filter  12] 

The  following  5  approaches  to  solve  the  target  tracking  problem  cire 
all  approaches  involving  some  sort  of  iteration  schema.  The  fjrst  approach, 
called  the  iterated  extended  Kalman-filter,  is  a  local  iteration  scheme, 
eind  is  designed  in  order  to  reduce  the  effect  of  measurement  function 
nonlinecirity.  This  approach  can  therefore  be  tried  on  the  Ceirtesian 
coordinate  system  representation,  which  have  linear  system  dynamics  and 
nonlinear  measurement  model. 

Since  this  approach  is  very  well  docinnented  in  Jazwinski  [2] ,  pp.  278- 
279,  it  should  be  unnecessary  to  repeat  the  equations  in  this  text.  For 
cC'Jipleteness ,  however,  the  approach  is  includec’  in  APPENDIX  C. 

3.5  Iterated  Linear  Filter-Smoother  [2] 

If  we  have  system  dynamics  nonlinearities,  the  preceding  iterator  will 
not  in^rove  the  estimate  due  to  system  nonlinearities  acting  on  the 

interval  [kT,  (k+l)T]. 

The  preceding  iterator  can  therefore  not  be  used  in  connection  with 
the  polar  coordinate  system  representation  of  the  target  motion. 
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The  "Iterated  Linear  Filter  Smoother",  proposed  by  Jazwinslci  12], 
pp.  279-281,  inclvtde  the  time  updating  process  in  the  iteration  loop. 

This  approach  can  therefore  be  tried  on  the  polar  coordinate  system  repre¬ 
sentation  of  our  tracking  problem. 

Again,  since  the  approach  is  well  documented  in  [2] ,  the  iterator 
equations  are  exiled  to  APPENDIX  D. 

3.6  Global  Iterated  Filters 

As  we  mentioned  in  Section  2,  one  of  the  main  problems  with  bearing 
only  tracking  is  the  poor  degree  of  observability.  The  observability  of 
range  is  entirely  dependent  on  the  maneuvering  scheme  of  the  observer. 

in  order  to  is^rove  the  observability,  and  at  the  same  time  i^pTOve 
the  reference  trajectory  and  thereby  get  a  more  optimal  utilizaticm  of 
each  observation,  a  number  of  global  iteration  schemes  are  proposed. 

The  iterations  will  here  not  be  performed  at  the  time  kT  (as  in  section 
3.4)  or  over  the  time  interval  [kT,  (k+l)T]  (as  in  section  3.5) ,  but  over 
the  greater  time  interval  [(k-N)T,  kTl ,  where  N  is  some  integer  constant, 
may  be  variable. 

This  iterator  roqpiires,  however,  a  lot  of  memory  capacity  for  storing 
of  variables.  The  following  sequences  has  to  be  stored  (M  is  an  integer 
consteuit  >  N) : 

Observation  Sequence: 


\  Vm+1'  \-M+2'*-"V 


(3.79) 
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Observer  position,  velocity  and  acceleration: 


k-M  k-M+l 


’^sk  “  '  -s  '  *  *  •  '-sk^ 

^  k-M  k-M+l  ^ 


^sk  “  ^-s  '  -s  '  *  * '  '-sk^ 

^  k-M  k-M+l  ^ 


covariance  matrix: 


\  “  '^\-M,k-M'  *  *  *  '\,k^ 


(3.80) 


(3.82) 


(3.83) 


As  we  can  see,  the  memory  capacity  requirements  will  depend  on  the 

size  of  the  integer  M.  Also,  if  the  iteration  time  interval  is  fixed 

(N  =  constant)  or  the  variation  space  for  N  is  fixed  (N,  <  N  < 

low  —  nXGn 

where  N  ,  and  N  are  constants) ,  the  requirements  for  storing  of  the 

iJJn  lllGH 

covariance  matrix  can  be  restricted  to 


P,  =  {P, 


k-N, 


HIGH 


,P 


k-N. 


LOW 


(3.84) 


The  requirements  concerning  storing  of  the  observer's  velocity  and  ac¬ 
celeration  will  also  depend  on  the  representation  of  the  tracking  problem. 
Equations  (3.79) -(3,83)  therefore  represent  an  upper  bound  on  the  memory 
capacity  requirements. 

Intuitively,  if  the  observer  has  performed  a  maneuvre  during  the 
time  interval  [ik-N)T,  kT] ,  a  global  iteration  scheme  will  in^rove  the 
observability.  The  idea  is  that  we  then  get  crossbearings  over  tliis 
time  interval. 

Three  different  approaches  to  the  Global  Iterated  Filter  will  be 
outlined  in  the  following. 
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3.6.1  Cartesian  Coordinate  System  Representation 

The  first  approach  to  be  Global  Iterated  Filter  utilize  the  Cartesieui 
CoorcTinate  system  representation  described  in  Section  3.1. 

The  nvanber  of  iterations,  i,  on  each  set  of  data,  has  to  be  limited. 
The  following  test  is  proposed  in  order  to  stop  the  iteration  sequence 
at  a  time  insteint  kT; 


(3.85) 


where  the  £  vector  has  to  be  selected  through  simulations  as  a  con^romise 
between  accuracy  and  coir^utertime  (number  of  iterations) . 

If  a  target  mcineuvre  is  detected,  the  size  of  the  iteration  interval 
should  be  decreased  in  order  to  not  perform  iterations  on  premameuvre 
target  data.  The  iterator  should  therefore  have  am  adaptive  calculation 
sequence  for  N,  in  connection  with  the  maneuvre  detection  system.  He 
will  return  to  this  point  when  dealing  with  the  maneuvre  detecticri  system 
in  Chapter  5. 

The  iterator  will  work  in  the  following  way: 

Having  performed  iterations  on  the  observation  sequence  {z. 

JC— 4&N 

V2B+1 . 11  is 

satisfied,  we  have  the  state  vector  covariamce  matrix 

P  .  .  These  are  stored  in  the  computer  memory.  Perform  the  global 

iteration  algorithm  subsequently  on  the  obseirvation  sequences  {z^^  2Mll'  ‘  *  ‘ ' 
\-N+l^'  ^*k-2N+2'*'*'  \-N+2^  fulfilled  iteration,  say 

at  san^lepoint  jT,  the  covariamce  matrix  P^  ^  and  the  state  vector 

X.  .  are  stored.  Like  in  the  conventional  Kalmam-f liter  case,  however, 

-3r3 

only  the  value  of  the  state  vector  at  the  last  sanplepoint  is  needed,  so 
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the  next  state  vector  x.  .  can  use  the  same  storage  are  as  x.  ,  .  ,  (and 

-1,1  -3-10-1 

thus  destroy  x .  ,  .  , ) • 

-3-1, 3-1 

Eventually,  when  we  arrive  at  san^le  kT,  we  have  the  following  data 
base  picture  (older  data  have  been  discarded) ; 


Observation  sequence ; 

covariance  aeqnence,  . "'k-l,k-r 


Observer  position  secfuence:  {x  ,...,x  .} 

“®k-N 


State  vector;  Bj,.  ^  1 


From  this  startpoint  the  following  calculations  are  performed: 


1. 

2. 


Time  updating. 


Calculate  x,  ,  _  and  P,  ,  , 
^,k-l  k,k-l 


Observation  integration. 

and  pf  ,  ,  where  i=l. 
k,k 


Process  z,  , 
k 


resulting  in 


k 


3 .  Tiraebackdating ; 


i-»,k-»  -  ^.k 

Pi  ev  1  e.  =  fetched  from  the  data  base 
k-N,k-N 


(3.86) 


4.  Reprocess  the  observation  sequence,  resulting  in 
$1+1  pl+1 
\,k'  k,k' 


5.  Perform  the  test  described  by  equation  (3.85)  to  decide 
whether  to  continue  the  iteration  loop,  or  to  stop  the 
iterations  on  this  obseirvation  sequence. 
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a.  If  decision  is  continue  the  iterations,  set  i=i+l, 
and  start  from  step  3  again. 

b.  If  decision  is  to  stop  the  iterations,  store 

i+1 

and  P  in  the  computer  memory,  set  k  =  k+1,  and 

JV  f  Jv 

stcurt  from  step  1  ageiin. 

This  iterator  will  successively  iii5)rove  the  linearization  trajectory 
for  Hj^  (given  by  equation  (3.10)).  The  utilization  of  the  data  sequence 
therefore  will  be  increasingly  more  optimal. 

3.6.2  Polar  Coordinate  System  Representation 

The  second  approach  to  the  Global  Iterated  Filter  utilize  the  Polar 
Coordinate  system  representation  described  in  section  3.2. 

The  mechanization  of  the  iterator  is  identical  to  the  iterator  in 
sequence  3.6.1,  except  for  the  time-back  dating  step. 

Due  to  the  nonlinearity  of  the  system  dynamics,  where  the  target 
position  information  is  given  in  relative  coordinates  (relative  to  the 
observer),  timeback  dating  by  use  of  the  inverse  form  of  equation  (3.25) 
(with  the  process  noise  vector  v.  =  0) ,  would  be  unnecessary  time- 

p 

consuming,  x,  .  would  then  have  to  be  calculated  for  decreasing  r,  i=k, 

3.,! 

k-l,...,k-N,  not  in  one  big  backdating  step  as  we  could  do  for  the  Cartesian 
Coordinate  system  model  (see  equation  (3.86)). 

In  order  to  save  con$»utertiffle ,  the  following  approach  for  time-back- 
dating  is  proposed: 

1.  The  state  vector's  position  information  is  transformeu  to 
Cartesian  representation  through: 


(3.87) 
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^,k  ^,k  *^k,k  ^  *sk 


yk,k  =  \,k  ‘^’k.k  +  ^sk 


(3.88) 


where  the  different  variables  are  defined  (without  time 
subscript  k,  though)  on  Fig.  3.1.  Since  the  velocity 
representation  is  equal  in  the  two  representations,  we 
now  have  a  complete,  Cartesian  state  vector  ^  j^. 

2.  Perform  time  backdating  through  equation  (3.86),  resulting 
“  ^-N,k-N. 

3.  Perform  the  treinsfonnation  from  Cartesian  to  Polar  repre¬ 
sentation  through  the  equations 


\-N,k-N  ■  \ 

=  tan  “ 

^k-N,k-N  \  y. 


'k-N,k-N“^s, 


(3.89) 


k-N 


\-N,k-N  ~  '^^\-N,k-N“^Sj^_jj^  ^^k-N,k-N"^Sj^_jj^  (3.90) 


Now,  if  the  time  back  dating  step  of  the  iterator  described  in 
section  3.6.1  (step  no.  3)  were  changed  to  include  processing  of  equations 
(3.87)  and  (3.88)  before  processing  of  equations  (3.86),  emd  to  include 
processing  of  equations  (3.89)  and  (3.90)  subsequent  to  equation  (3.86), 
the  calculation  steps  1-5  given  in  section  3.6.1  will  be  valid  also  for 
the  Polar  coordinate  system  case. 

One  additional  preferable  requirement  exists,  however:  The  data¬ 
base  should  also  contain  the  observer's  incremental  position  change  from 
sample  to  san^le,  given  by  the  left  hand  side  of  equation  (3.24),  over 
the  time  interval  [(k-N)T,  kT] . 
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Alternatively,  these  quantities  can  be  calculated  frcm  the  sequences 
given  in  equations  (3.81)  and  (3.82) ,  if  these  axe  in  the  database. 

This  iterator  will  successively  improve  the  linearization  trajectory 
for  (given  by  equations  (3. 31) -(3. 32) ) .  The  computational  require¬ 
ments  will,  however,  exceed  the  iterator  in  Section  3.6.1. 


3.6.3  "Serial"  filters 

If  the  two  previous  described  global  iterators  process  each  observa¬ 
tion  sequence  ^-N+l'*"'\^  twice,  which  is  the  minimun  nunber 

of  iterations,  each  individual  observation  z^  will  be  processed  2N 
times.  It  is  thus  obvious  that  these  iterators  will  increase  the 
con^utational  burden  very  heavily,  as  compared  to  the  Extended  Kalman- 
filter  case. 

In  an  atten^t  to  reduce  the  calculation  load,  and  still  bring  along 
the  advantages  of  the  global  iteration  approach,  a  different  iterator 

roposed  for  the  Cartesian  system  representation.  The  effect  of  this 
in  fact  two  coupled  filters,  running  along  the  target 

_ .  „uory  with  a  certain  time  delay,  NT,  between  the  filters.  Ibis  is 

the  reason  for  the  name  serial  in  the  heading  of  this  section. 

The  two  serial  filters  are  added  together  in  one  system  description. 


in  order  to  get  a  cosset  form.  We  have: 
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0 

0 

t2/2 

0 

0 
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0 

0 

0 
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0 

0 

^2k 

0 

0 

t2/2 

0 

• 

V 

Ik-N 

0 

0 

0 

t2/2 

0 

0 

T 

0 

V 

2k-N 

0 

0 

1  0 

T 

(3.91) 


“k-N 


tain 


tan 


w. 


w, 


k-N 


(3.92) 


k-N/ J 

In  equations  (3.91)  and  (3.92),  N  is  the  nvnnber  of  samples  between  the 
two  pairts  of  the  state  vector. 

We  now  define: 


Px 

-Ik 

■ 

_-2k 

_^-N 

and 


■ - 1 

Uk. 

Then,  equation  (3.91)  can  be  written: 


^+1 


0 

<j)((N+l)T) 

e(T) 

0 

1 

<i>(-(N-l)T) 

0 

e(T) 

(3.93) 


(3.94) 


(3.95) 
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Further  defixilng: 


the  observation  equation  (3.92)  can  be  written; 


(3.96) 


(3.97) 


(3.98) 


(3.100) 

(3.101) 


(3.102) 
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^(T) 


0 

(}»((N+1)T) 

({)(-(N-l)T) 

m 

0 

J 

(3.103) 


and 


(3.104) 


where  and  are  given  by  equations  (3.8)  and  (3.9),  0(T)  by  equation 
(3.6)  and  (J)(T)  by  equation  (3.5).  can  also  be  written: 


\  j  ° 

“  1  J 


(.1.105) 


where  is  given  by  equation  (3.10). 

The  Extended  Kalman  filter  equations  for  this  augmented  (•)  system, 
are  given  by  equations  (3 .11) - (3.18) ,  if  replacement  with  the  augmented 
variables  are  performed  in  the  equations. 

In  order  to  start  up  the  augmented  system  correctly,  the  first  N 
observations  are  processed  with  the  Kalman-filter  given  in  section  3.1. 
Then  the  initial  values  for  the  augmented  system  will  be; 


2o,-l 


^,k 


(3.106) 


■0,-1 


k,k 


Lo 


o-* 


(3.107) 
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At  this  point  it  is  appropriate  with  a  few  connents  on  this 
approach  to  global  iteration.  As  we  can  see  from  equation  (3.95),  the 
first  part  of  the  state  vector,  is  updated  on  the  basis  of  the 

second  part  of  the  state  vector,  *2^'  vice  versa.  From  equation  (3.92) 
we  can  further  deduce  that  each  observation  will  be  integrated  twice. 

Looking  at  the  iteration  scheme  given  in  section  3.6.1,  we 
can  see  that  our  ' serial"  filter  is  equivalent  with  the  steps  1-4, 
with  step  4  reduced  to  integration  of  followed  by  a  time  updating 

step  up  to  time  (k+l)T.  Step  5  does  not  exist,  each  observation  is  only 
reprocessed  once. 

The  global  iteration  effect  of  this  approach  should  then  have  been 
demonstrated. 

It  should  be  pointed  out  that  this  scheme  can  be  augmented  to 
incorixirate  a  selected  number  of  filters,  say  M,  following  each  other 
with  a  time  difference  NT.  The  resulting  model  would  then  look  like: 


(k+1)  =  ' 

1 

0 


0  4i((N+1)T)  0 

„  N  N 

9  N 

1  \  \ 

I  N  \ 

I  N  \ 

^  ^V^<j)((^l)T) 

(})(-(MN-l)T)  0 - 0 


»1 


0(T)  0 

\ 


.  0  0  {T)_ 


with  an  observation  equation: 
r  n  r.._-lMx 


(r,  <^>) 


z(k-MNy  Itan"^^^  (k-MN)^J  |_w(k-l 


+  . 


(3.108) 


(3.109) 
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In  this  case,  each  observation  will  be  processed  M  times. 


3.7  Parallel  Filter  Approach 

The  last  approach  that  will  be  explored  is  the  parallel  filter 
approach.  In  this  case,  where  we  have  M  parallel  filters  with  different 
linearization  trajectories,  only  the  sinplest  Cartesian  model  for  each 
filter  will  be  considered,  in  order  to  reduce  the  computer  time.  Each 
filter  will,  therefore,  be  represented  by  the  mathematical  model  given  by 
equations  (3.3)-{3.7). 

The  philosophy  behind  this  approach  is  the  following;  Each  filter 
will  be  ini-tialized  with  different  range,  R  .,  i=l,2,...,M.  The  initial 

Oi 

values  for  the  velocity  components  for  each  filter  will  depend  on  R^, 
if  the  initialization  routine  proposed  in  Chapter  4  is  used. 

In  the  time  interval  from  initialization  until  the  first  observer 
maneuvre,  target  range  is  unobservable.  The  utilization  of  the  bearing 
observations  in  this  time  interval  will  be  nonoptimal  for  all  the  filters 
except  the  one(s)  with  approximately  correct  range.  Ifhen  the  observer 
performs  a  maneuvre,  target  range  becomes  observable.  It  is  then  possible 
to  identify  which  of  the  “  filters  that  has  the  most  correct  range. 

The  resulting  state  vector  for  this  filtering  scheme  can  be  given 
by  [14]; 


4  “  ",  '“'\i '  W  ■  4i 

1=1 


where  the  probabilities  P 
The  question  is  how. 


i=l. 


,M,  have  to  be  calculated. 
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Now,  the  most  likeQ.y  qioantity  to  contain  information  about  thete 
probabilities,  are  the  innovation  sequences  for  each  filter.  These  are 
given  by: 


^i  ~  \  \,k-] 


i=l,2,. • .  ,M 


(3.111) 


In  our  special  case,  is  a  scalar  variable.  Under  a  number  of  conditions, 

which  include  the  re^ ’irement  of  equality  between  the  physical  system  and  the 

mathematical  model  contained  in  the  Kalman-f ilter ,  the  innovation  sequence 

2 

has  been  shown  to  be  a  white  Gaussian  sequence  with  statestics  ~N(0,  » 

2 

where  a  is  given  by: 


“  \,k-l\,k-l“k,k-l  \ 


(3.112) 


2 

i=l,2,...,M,  are  already  calculated  by  the  Kalman  filtfu:  algorithm. 

2 

=  0,  cind  i=l,2,...,M,  represents  the  expected  mean  and  variance 

for  the  innovation  sequence. 

The  expected  variance  of  the  innovation  sequence  given  by  equation 
(3.112)  will  be  different  for  each  filter,  i.e.,  the  variance  will  depend 
on  R^.  Equation  (3.112)  can  be  written: 


=  y  '4  ^ *  '■22,  ^  4,k-i-4  ^ 

R.  k,k-i  k,k-l  k,k-l 

1 


or  equivalently 


(3.113) 


1  ,„i 

^ki  ~  2  ^11 

RT  k,k-l 
1 


*  4  ^  ”lc 

k,k-l  1 


(3.114) 


where 
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1  i 


“r*  ♦k.k-!  +  ^22^  ,  ♦ic.k-l  ■*  'l2,  ,  ,' 

1  k,k-l  k,k-l  k,k-l 


sin2(]>. 


k,k-l 
(3.115) 


The  difference  between  the  M  different  will  not  be  significant 

in  this  caset  since  the  observability  of  the  bearing,  (P,  is  very  high. 

The  values  of  the  covariance  matrix  elements,  ^22  ^12' 

will  depend  on  R. .  In  the  equations  for  calculation  of  P  ,  ,  the  observation 

X  iC  f  JC 

2 

noise  will  be  weighted  by  R^.  See  Appendix  E  for  detailed  derivation  of 
the  covariance  equations. 

As  can  be  seen  from  Appendix  E,  each  of  the  elements  of  the  covariance 
matrix  P.  .  will  increase  for  increasing  R. .  The  value  of  the  e:q>ected 

jC  ^  jC  J. 

innovation  variance  given  by  equation  (3.113)  will,  therefore,  not  decrease 
-2  2 

as  a  function  of  R,  ,  however,  0,  .  will  decrease  as  some  function  of  R. , 
r  ki  1 

since  the  elements  of  P,  are  limited  in  their  growth,  and  does  not  in- 

iC  I  iC 

2 

crease  with  the  power  of  R^  anyway.  A  detailed  simulation  analysis  of  the 

equations  given  in  Appendix  E  has  to  be  Ccurried  out,  in  order  to  reveal 

the  exact  behaviour  of  P,  ,  as  a  function  of  R.. 

k,k  i 


The  actual  statistics  for  each  filter  i,  i=l,2,...,M,  can  be  approximated 


by; 


m_ 


'ki 


1 

N+1 


j^N 


e. . 

31 


(3.116) 


‘^e  ~  mi  E 

j=k-N 


(3.117) 


The  idea  is  now  to  compare  the  innovation  sequence ' s  actual  statistics 
with  its  expected  statistics,  and  thereby  get  an  expression  for  the 
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probabilities  P(Rj^  =  * 

In  doing  this^  we  have  to  consider  the  following  two  cases: 

1.  System  not  observable  in  the  time  interval  [(k-N)T,  kT] . 

2.  System  is  observation  in  the  time  interval  [{k-N)T,  kT] . 

By  observable  or  not  we  mean  whether  or  not  the  range/velocity 
ambiguity  can  be  i»-jolved  through  the  bearing  observation  over  the  time 
interval  [(k-N)T,  kT] .  In  any  case,  the  bearing  to  target  will  be 
observable,  and  so  will  the  ratio  Av/R,  where  Av  is  the  relative  velocity 
between  target  and  obsezrver. 

The  information  as  to  whether  the  system  is  obf\ervable  or  not,  2uid 
the  degree  of  x.he  observability,  is  known  to  the  tracking  routine, 
since  the  observer's  mcuieuvering  history  in  the  time  interval  [(k-N)T,  kT] 
is  known. 


3.7.1  System  not  observable. 

If  the  observer's  velocity-  end/or  course-changes  are  below  defined 
thresholds  over  the  time  interval  in  question,  we  know  that  the  range/ 
velocity  ambiguity  Ccin  not  be  resolved  from  bearing  observations  only. 

This  case  can  further  be  devided  into  the  following  two  std>-cases: 

1.  The  target  is  not  maneuvering  during  the  time  interval. 

2.  The  target  is  maneuvering  during  the  time  interval. 

A  maneuver  is  defined  as  a  definite  course  and/or  velocity  change, 
not  the  natural  small  fluctuations  in  velocity  and  coiurse  due  to  waves. 


wind,  etc. 
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3 . 7 . 1 . 1  Target  not  Maneuvering 

If  the  target  is  moving  along  a  straight  course  with  constant  speed, 
the  actual  mean  value  of  the  innovation  signal  given  bv  equation  (3.1165 
will  be  close  to  zero  (below  a  defined  threshold,  say  • 

Since  we  know  that  the  target's  natural  fluctuations  about  some 
mean  course  and  speed  doesn't  depend  on  the  distance  from  where  it  is 
observed,  it  is  likely  that  the  bearing  statistics  should  contain  some 
range  information. 

In  fact,  this  case  looks  like  the  stationary  process  case  where 
adaptive  noise  estimation  is  possible.  See  Mehra  [15] ,  [16] ,  and  Chin 
[17],  [18]. 


In  this  case  we  propose  to  use  the  covariance  matching  method, 
however,  not  to  do  any  adaption  (even  if  that  should  also  be  possible) , 


but  to  arrive  at  an  expression  for  the  probability  function  p(R.  =  R/z  ) . 

X  iC 

If  we  set  the  equations  (3.114)  and  (3.117)  equal  (expected  variance 
actual  vciricince) ,  we  get: 


~(P^ 

2 '  11 

R.  k,k-l 
1 


(R.) 


+  P 


22 


k,k-l 


(R,)  - 


+  \  = 


(3.118) 


'k. 

1 


Solving  this  equation  for  the  only  explictly  occurring  R^  in  the  equation 
gives  us: 


R*  = 
1 


!P^,  (R.)  +  P^n  (R.)-< 

llj^  k-1  ^  ^^k  k-1  ®i 

' 

1 


where  R*  may  be  different  from  R.. 
1  1 


(3.119) 


The  difference: 
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AR.  =  R*  -  R. ,  i=l,2 
111 


(3.120) 


will  be  an  ej^ression  for  the  range  error  for  each  filter. 

We  propose  the  following  form  for  the  probability  function 
p(ft^=R/Zj^) ; 


-o-Ar^ 

p(R^  =  R/z^.)  =  K  •  e  ^ 


(3.121) 


where  the  variables  K  and  a  will  depend  on  the  process  noise  covariance 
matrix  V^,  and  have  to  be  decided  upon  through  simulations. 


3. 7. 1.2  Target  Maneuvering 

During  a  target  maneuvre,  the  method  described  in  section  3. 7. 1.1  will 
not  work,  since  we  in  this  case  gets  temporary  changes  of  unknown  size 
and  duration  in  the  process  noise  coveiriance  matrix  Vj^ ,  that  are  not 
taken  into  account  in  the  calculation  of  the  expected  variance  of  the 
innovation  signal. 

Therefore,  when  a  target  maneuvre  is  detected,  for  example  by  an 

abriipt  change  in  both  the  actual  mean  and  variance  of  the  innovation 

signal,  the  last  calculated  probabilities  p(R^  =  R/Zj^)  should  be  used, 

until  the  filters  have  adapted  to  the  new  course  and/or  speed,  and 

m^  again  drops  below  the  threshold  ni  . 

^i 

Alternatively,  if  it  is  possible  to  decide  upon  the  time  delay 
between  the  instant  of  the  target  maneuvre  and  its  detection,  the 
probabilities  P(R^  =  slKJuld  be  used,  where  At  is  the  described 

time  delay. 

Target  maneuvre  detection  will  be  treated  in  Chapter  5. 
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3.7.2  System  Observable 

If  the  observer  is  maneuvering  during  the  time  interval  [(k-N)T,  kT] , 
the  range/velocity  ambiguity  can  be  resolved  based  on  the  bearing 
observations  only.  In  this  case  the  filter  with  the  most  correct  range 
can  be  identified,  if  the  target  is  not  maneuvering  during  the  time 
interval  in  question. 

We  therefore  have  to  consider  the  same  two  sub-cases  as  in  section 
3.7.1: 

1.  The  target  is  not  maneuvering  during  the  time  interval. 

2.  The  tcirget  is  mcineuvering  during  the  time  interval. 

3. 7. 2.1  Target  not  maneuvering 

The  filter  with  correct  range  will  not  change  its  actual  statistics 

when  the  observer  performs  a  maneuvre.  This  fact  can  be  utilized  to 

prune  off  the  filters  with  wrong  range. 

2 

By  monitoring  m  and  o  given  by  equations  (3.116)  and  (3.117)  through  an 

^i 

observer  iticineuvre,  it  is  thus  possible  to  identify  the  filter  with  the 
best  range  as  the  filter  with  the  least  change  in  actual  statistics. 

In  this  case,  we  continue  to  calculate  the  probability  functions 
given  by  equation  (3.121)  for  a  gradually  decreasing  number  of  parallel 
filters,  as  the  filters  with  the  wrongest  ranges  successively  will  be 
removed  from  being  in  an  active  state  to  a  so-called  "dormant"  state, 
where  they  no  longer  are  updated. 

The  selection  of  the  filters  with  bad  ranges  can  be  made  from  monitoring 

m  .  When  m  exceeds  a  given  threshold  m  ,  the  filter  no.  i  is  temporarily 
ki  ki 
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pruned  off  the  ensemble. 

In  this  way  the  number  of  necessaary  parallel  filters  can  be  reduced, 
and  if  the  observer  maneuvre  extends  over  a  time  interval  long  enough,  or 
if  the  observer  performs  successive  maneuves  while  the  target  remains  on 
straight  course  with  constant  speed,  we  will  eventually  be  left  with  one 
filter,  with  correct  range. 

This  situatx  n  will  prevail  only  until  a  target  maneuvre  is  detected, 
when  all  the  M  parallel  filters  should  be  initialized  again. 


3. 7. 2. 2  Target  maneuvering 

When  a  target  maneuvre  is  detected,  all  the  M  filters  should  be  re¬ 
initialized,  independent  of  the  number  of  active  filters  at  the  time  of 
rocineuvre  detection. 

The  M  filters  should  be  reinitialized  with  different  ranges 
centered  about  the  estimated  range  ^  given  by  the  state  vector 
^  at  the  time  of  maneuvre  detection.  The  difference  between  the 
individual  should  be  much  less  than  during  the  first  initializa¬ 

tion,  since  range  is  not  at  all  so  imcertain  this  time. 

Simultaneously,  the  velocity  elements  of  the  covariance  matrix  should 
be  increased,  in  order  to  allow  the  filter  to  adapt  to  the  targets  new 
course  and  speed. 

After  reinitialization,  the  calculation  of  the  probability  function 
given  by  equation  (3.121)  is  resumed. 

Target  maneuvre  detection  and  handling  in  connection  with  parallel 


filters  are  treated  in  section  5.3. 
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3.7.3  Closing  remarks,  parallel  filters. 

The  parallel  filter  approach  seems  very  promising,  however,  the  calcu¬ 
lation  load  iitposed  by  this  approach  will  be  substantial. 

In  order  to  arrive  at  a  practical  parallel  filter  solution,  the 
idea  of  adaptively  reducing  the  number  of  parallel  filters  have  to  be 
investigated  thoroughly.  This  can  only  be  done  through  a  simulation 
study. 

Further,  this  approach  claim  for  a  substantial  simulation  study  in 
order  to  "tune"  in  the  different  thresholds^  time  intervals  and  variables 
defined  in  the  previous  sections. 

One  fundamental  difference  between  the  parallel  filter  approach  and 
the  single  filter  approacl  ,  is  the  range  covariance.  For  the  parallel 
filter  approach  the  range  covariance  should  be  kept  low,  in  order  to 
make  each  filter  "stiff"  in  range.  This  is  not  normally  the  case  for  the 
single  filter,  where  range  covariance  has  to  be  high  in  order  to  allow  the 
filter  to  adopt  the  correct  range  during  the  periods  when  range  is  ob¬ 


servable  . 


4.  INITII^IZATION  HDUm^E 


4.1  General 

Bearing  only  tracking  from  a  single,  moving  observer  is,  in  the 
initialization  phase,  very  dependent  on  the  selected  initial  values  of 
range,  course  and  speed  of  the  tcurget. 

The  initial  values  of  the  elements  of  the  covariance  matrix  for 
the  estimation  error  have  also  an  in^xartant  influence  on  the  initial 
track. 

One  method  commonly  used  for  selecting  initial  data  for  bearing 
only  trackers,  is  to  utilize  the  knowledge  of  range  reach  of  the  bearing 
sensor.  The  argument  is  the  following:  If  a  teurget  is  detected  at  some 
time  Tjj,  that  could  not  be  "seen"  by  the  sensor  at  times  <  Tj^,  the 
reason  is  that  the  target  hcis  just  entered  the  reach  area  of  the  sensor, 
and  is  opposing  the  observer.  The  initial  range  is  therefore  given  by 
the  sensor  specifications,  and  the  initial  covirse  is  towards  the  observer 
(Initial  velocity  still  has  to  be  picked  out  of  the  air) . 

Since  the  range  reach  of  the  sensor  usually  depends  on  the  environ¬ 
mental  conditions,  this  method  frequently  will  give  very  bad  initial  data. 

The  purpose  of  this  chapter  is  to  derive  an  alternative  method,  where 
initial  data  can  be  selected  in  a  more  optimal  manner,  based  on  calcu¬ 
lations  performed  on  a  given  set  of  bearing  observations  from  the  moving 
obsejrver,  and  the  position  history  of  the  observer. 

The  following  derivation  will  also  give  us  some  valuable  insight 
in  the  observability  problem,  eind  show  the  conditions  under  which  the 
range/velocity  ambiguity  can  be  resolved  based  on  bearing  observations 
only. 
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4.2  Geometric  Problem  Visualization 
A  typical  geometric  sitviation  is  depicted  in  Fig.  4.1. 


A  moving  observer,  0,  observes  a  number  of  bearings  to  the  target, 
T,  at  the  time  points  t^,  t^,  t^,...,  where  the  time  difference  between 
any  two  consecutive  bearings  may  be  different. 

The  following  assumptions  are  made: 

1.  The  target  is  moving  with  constant  course  amd  speed. 

2.  The  observer's  velocity  and  course  are  constant  between 
the  observations.  (This  restriction  will  be  removed 
later) . 

3.  The  bearings  are  noise  free. 

Assumption  number  3  is  obviously  not  true  in  reality.  However,  by 
use  of  this  assumption  it  is  possible  to  arrive  at  certain  results. 


-50- 


In  the  discussion  of  these  results  later  in  this  chapter,  methods  for 
reducing  the  effect  of  this  assumption  being  violated  will  be  suggested. 


4.3  Calculation  of  target  velocity  components. 

Based  on  three  consecutive  bearings  (j)^,  ()>^,  and  an  assumed 
start  range,  R^,  the  teurget  velocity  components  can  be  calculated.  We 
have  the  followl  three  equations  (see  also  equation  (3.25)): 


Al.jj(Atj^) 

v^p(^h) 


tan((l)j^-(|)Q) 


(4.1) 


(R-+AL^(At,))‘ 

0  p  1 


(4.2) 


Al^(At2) 

R.  +  AL  (At_)  “  tan(4,2-‘({)j^) 

1  p  2 


where 


N  =  V‘o 


*<=2  =  ‘2-‘l 


or  generally 


At. 

1 


=  t.-t 
1 


i-1 


9 


i=l,2 


and 


(4.3) 

(4.4) 

(4.5) 

(4.6) 


AI^(A- 


ti) 


cos  (|),  ,  [v  At  -Ax 


]  -  sin  At^  -  Ay^  ]  (4.7) 

i-1  ^  i-1 


Al  (At.)  =  sin  (|).  .  [v  At. -Ax  ]  +  cos  (fi.  [v  At. -Ay  ]  (4.8) 

p  1  1-1  XI  s.  ,  1-1  y  1  ■'s.  , 


In  equations  (4.7)  cind  (4.8) ,  assun^tions  1  and  2  from  section  4.2  are 
used.  The  target  movement  along  x-  and  y-directions  in  the  time  inter¬ 
val  At.  are  given  by  v  At.  and  v  -At.  respectively  (no  acceleration 
1  XI  yi 

term) ,  while  the  observer  movement  along  the  same  eixis  in  the  time 
interval  are  given  by: 


1  2 
Ax  =  V  .At.  +  —  a  At. 
s.  T  sx.  ,  1  2  sx.  -  1 

1-1  1-1  1-1 


Ay  =  V  At.  +  ^  a  At? 


(4.10) 


If  the  observer  position  increments  in  the  time  interval  At^  is  calculated 

on  the  basis  of  equations  (4.9)  and  (4.10),  assumption  2  is  necessary. 

However,  by  deviding  the  time  interval  At^  into  smaller  intervals, 

and  calculating  Ax  and  Ay  as  a  summation  of  position  increments 

i-1  ^i-1 

over  these  inteivals,  provided  that  velocity  and  acceleration  data  for 
the  observer  in  these  intervals  exists,  the  assumption  of  constant 
velocity  and  course  can  be  changed  to  yield  arbitrarily  small  time 


increments. 


Now,  from  equation  (4.1)  we  have: 


ALjj(Atj^)  =  tan(4)^-(t)Q)  (E^  +  AL  (At^^)) 


(4.11) 


Inserting  from  equation  (4.11)  into  equation  (4.2)  gives: 


=  (Rq  +  Al  (Atj^))  .  ’-^1  +  tan  ((t)j^-<l>Q) 


(4.12) 
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Then; 


=  (Rq  +  ALp(At^))/cos(<t>j^-<i)Q) 


(4.13) 


Further; 


R^+sin  ^  ^ 


cos  (<t>j^-<l>Q) 


(4.14) 


Inserting  for  equation  (4.1),  gives,  after 


p— 1' 


some  calculation: 


1  1  ^'1^1 

V  =  7^  Ax  +  tan  4i-,  (v  -  77-  Ay  )  +  7- - 7 — 

Ati  Sq  1  y  At^  -^Sq  At^  cos  (|)^ 


(4.15) 


Now,  inserting  for  from  equation  (4.15)  in  equation  (4.14)  gives: 


%  cos  <t»C,  •«•  (VyAt^-Aygp) 

cos  <()t 


(4.16) 


We  also  insert  for  v^  from  equation  (4.15)  in  the  equations  for 
A]:4jj(At2)  ar.d  ALp(At2)  (equations  (4.7)  and  (4.8),  with  i=2) .  The 
results  are: 


“2  /*’'2 


■  At, 


-  sin 


♦i(h:  ‘’'so  -  ‘''si) 


(4.17) 


^^2  ^^2 

(J.^-sin«t>^-<DQ)  t  sin  ({.^(  ^  Ax^o-^^1^ 


At, 


cos 


7—  V - i-7-  (7-^  Ay  -  .sin^(j),+Ay  .cos^i  )  (4.18) 

y  cos  (j)j^  vAtj^  ■'sO  ^1  ^sl  ^1/ 
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From  equations  (4.17)  and  (4.18)  we  find  that: 


=  Al^(At2).tan 


(4.19) 


Then  we  have: 


R.,+Al  (At.)  = 

1  p  2 


Rq  COS4.Q  +  AL^(At2)sin(()j^  +  v  (Atj^+At2)-(AygQ+Ayg^) 


cos  (j) 


(4.20) 


Inserting  for  (R^+ALp(At2) )  from  equation  (4.20)  in  equation  (4.3)  gives: 
AL^(At  Jcosv}) 

=  “o  ♦o  *  'I'l  + 


(4.21) 


-(Ay  -+Ay  ,) 
•'sO  "^sl 


Since  ALjj(At2)  given  by  equation  (4.17)  is  not  a  function  of  v  ,  and 


iti+At^  =  tj-tg 


(4.22) 


equation  (4.21)  can  be  solved  for  v^.  The  result  is: 


1  l\i^^t^)cos  <t>2 


V  =  T — r"  - : — 72 — r“^ - Ro  cos  (j>*  +  Ay  +  Ay 

y  t2”^0  Sin(<{)2-4'j^)  0  ^0  ■'sO  ■'si 


(4.23) 


Inserting  for  AlYj(At2)  from  eq’uation  (4.17)  gives  our  final  result  for 


(At  sin (<})  -({)  ) cos  (}),  ) 

V  =  :r  Rn)  aZ^  - ■  TT  2-, - ^  *  cos  ({)-  Ay  -  +  Ay  ,  + 

y  ^2"^0  sin  (({>2-<{>-j^)  0)  sO  ■'si 


cos  ({>2  /At2  \  /At^ 


(4.24) 
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Lastly,  inserting  for  from  equation  (4.24)  in  equation  (4.15),  gives 
our  final  result  for  v^: 


'  ‘2"‘o 


At^  sin  (<j)j^-(|)Q)sin02 
At, 


s;, 


-  3in(|). 


+  Ax  -  +  Ax^,  + 
sO  si 


sin4). 


sin((j)2-<l>i 


(4.25) 


Equations  (4.24)  and  (4.25)  are  the  main  results  in  this  section.  We 
are  interested  in  their  form  for  a  cov^le  of  special  cases: 


Case  1: 


Atj^  *  At2  “  At 


Observer  non-maneuvering,  i.e.: 


Ax.,  =  Ax-=v  "At 
sO  si  sx 


Ay  ^  =  Ay  ,  =  v„„*At 
•^sO  -^sl  sy 


(4.26) 


Q  ^  sin(<|)j^-<tiQ)sin(j). 

^At  I  sin  (({»2'*4>j^) 

Rq  |sin(4)^-({'Q)cos 

TAt  I  sin(^p^ 


V  =  V  +  -r 
X  sx  2 


V  =  V  +  -r 

y  sy  2 


-  sin  !|>. 


-  cos  <|) 


.1 


(4.27) 

(4.28) 

(4.29) 

(4.30) 


As  we  can  see  from  equation  (4.29)  and  (4,30) ,  the  target  velocities 
that  can  be  determined  on  the  basis  of  ai^  three  consecutive  bearings 
from  a  non-memeuvering  target,  will  depend  on  the  range  Rq  (can  be 
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transformed  to  yield  by  solving  eqxaation  (4.16)  for  after  in¬ 
serting  for  Vy.  Similarily,  the  range  dependence  can  also  be  transformed 
to  R^) . 

It  is  important  to  realize  that  the  same  dependence  on  range  exists 
when  the  target  velocities  are  calcxilated  by  the  Kalman  filter.  This  is  the 
range/velocity  ambiguity  in  a  nut-shell! 

Case  2i 

At^  =  At2  =  At 

Observer  not  moving  at  all,  i.e.: 


S”*”  Uy)~  \sin(*j-4.g)cc.s*2-sitl(4.2-+i>=°='t'o  /  ' 

The  target  velocity,  however,  will  still  depend  on  the  range. 

These  two  special  cases  also  can  be  used  to  give  guidelines  for  an 
intelligent  observer  maneuver  strategy;  Start  tracking  while  the 
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observer  stays  put,  and  determine  the  target  course.  When  the  observer 
starts  moving,  preserve  the  detesmdned  target  course  in  the  Kalman  filter 
(by  keeping  lew,  may  be  artifically) .  Then  the  filter  will  quickly 
adopt  the  correct  range.  An  indication  of  this  cein  be  found  from 
equations  (4.29)  emd  (4.30).  We  can  write: 


V  V  +  R-K, 

.  _  X  sx  1 

tan  C„  =  — 

T  V 

y 


'  +  R*K^ 

sy  2 


(4.36) 


where  is  assumed  known,  and  and  are  constants,  independent  of 
range.  Equation  (4.36)  cein  be  solved  for  R,  to  give: 


R  = 


V  -  V  •  tan  C 
sx  sy _ T 

K • tanc  -  K 
2  T  1 


(4.37) 


4.4  Calculation  of  Initial  Range 

By  inclusion  of  a  4th  bearing  observation,  the  results  of  the  previous 
section  can  be  extended  to  include  the  determination  of  the  initial 
range,  provided  the  observer ^ is  maneuvering  during  the  observation 
period. 

When  a  4th  bearing  is  included,  the  following  equation  can  be 
included: 


tan(4i2-<t>2^ 


Here,  R^  is  given  by; 


(4.38) 


>/(R^+ALp(At2))^  +(A]^(At2))^ 


(4.39) 


-57- 


il 


I  i 


I  I 


i  R 

t 


K  t 


I  ! 


Since,  from  equation  (4.3): 


ALjj(At2)  =  (Rj^+Al  (At2))tan((j)2-4>j^) 


e<xuation  (4.39)  can  be  written: 


^2  ~  /cos  ((jj^-'Jij^) 


Now,  by  use  of  equation  (4.20)  we  get: 


(4.40) 


(4.41) 


^2  = 


yos^O  +  %(At2)sin4)^  +  Vy(Atj^+At2)-(Ay^Q-fAy^j^) 
cos  cos(<j)2-<j>2) 


14.42) 


The  next  step  is  to  insert  for  Al<jj(At2)  from  equation  (4.17)  euid  for  from 
equation  (4.24)  in  equation  (4.42).  After  some  manipulations,  we  get  the 
following  result: 

At„ 


^2  = 


^  RQsin(({)j^-!})Q)  +  cos 


♦1(3?^  '^%o-  '^Si)  '  <’i(a^J'^o-'^ysi) 


sin(<j)2-<l>j^) 


(4.43) 


Next,  the  two  other  variables  in  equation  (4.38),  AL^(At2)  and  ALp(At2) 
have  to  be  calculated.  We  have: 


Al^(At3)  =  cos  <l>2[v/t3  -  Ax^2^  '  **>2  ^ V^3"^ys2’ 


(4.44) 


ALp(At3)  =  sin  <l>2[v/t3  -  Ax^2^  +  <*>2  ^ V^3"^ys2’ 


(4.45) 


ii 

I  ff 

6  f; 

Ml 


fe  ter- 


s  « 


F\u:ther: 


-  %“'3>  ■  “"♦2  * 


(4.46) 
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NOW,  inserting  for  AL^CAt^)  from  equation  (4.46)  in  equation  (4.38)  gives, 
after  some  rearrangements: 


cos  <|) 

'  ■'2  “='^2  *  V*3  -  ‘‘^b2 


(4.47) 


Next,  we  have  to  develop  AL^(At2)  further.  Inserting  for  v^  and  v^  from 
equations  (4.25)  a.  '  (4.24)  in  equation  (4.44)  gives,  after  some  calculation; 


V^3 

- sin(<|:2-<|)Q)  +  cos  (p^ 


At. 


(4.48) 


-  sin(j). 


At, 


L  2  "0 


Lastly,  by  inserting  for  from  equation  (4.48),  for  from  aqi.ation 

(4.43)  and  for  v^  from  eqiiation  (4.24),  equation  (4.47)  cjin  be  solved  for 
Rq.  The  result  is; 


^0  = 


sin  ♦  At^{sin(|)3  (At3  (Ay^^+  y^^)  -  (t^-t^)  Ay^^)  -cos(})3  (At3  (Ax^q4 

At^-At2*sin(4)2-<l>Q)sin(<})„-({)^)  - 


sin(4)3-({>^)(t3-tQ)  |cos(j)^ (At^Ax^p-At^Ax^^) -sincj)^ (At^Ay^Q-At^Ay^^! 
At^  (t3-tQ)  sin  sin  {<l>3-'}>2) 

(4.49) 

Equation  (4.49)  is  the  final  result  in  this  section.  An  important  special  case 
is  obtained  when: 

At^  =  At2  =  At^  =  At 

tj-t^  =  2At 


‘3-S  ' 


(4.SU) 
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Then,  equation  (4.49)  can  be  written; 


sin  (({>^-(1)^)  {sin4»3  ^2'^  -cos(t>3  ^ 

sin  (<1>3-<1>q)  sin  "3  sin  (4>2^~<|)q)  sin  (4>3-<!>2) 


3  sin  (<j>3-<l>2)  •  ^cos(|)J^  (Ax^q-Ax^^)  -sin(j)j^  ^ 


(4.51) 


If  the  observer  is  not  maneuvering,  we  have: 


Ax  -  =  Ax  ,  =  Ax  ... 
sO  si  s2 


(4.52) 


'  ''i'si  ■  Ki 


From  equation  (4.51)  it  is  then  easy  to  see  that  Rq=0»  meaning  that  range 
cein  not  be  observed  with  a  nonmeaneuvering  observer. 

When  range  is  calcul.^ted  from  equation  (4.49)  or  (4.51),  equations 
(4.24)  and  (4.25)  can  be  used  with  this  range  to  calculate  the 
target  velocity  components . 

Appendix  F  gives  a  nianerical  example  on  the  use  of  the  results  in 
section  4.3  and  4.4. 


4.5  State  vector  initialization. 

Due  to  obvious  reasons,  the  single  Kalman  filter  case  and  the  parallel 
filter  case  have  to  be  treated  separately  under  this  heading. 


4.5.1  Single  Kalman  filter  case. 

The  results  in  Section  4.3  and  4.4  are  based  on  the  assumption  of 
noisefree  bearings.  They  can,  however,  be  used  directly  as  initial  data, 
with  the  associated  initial  values  for  the  covariance  matrix  as  given  in 


section  4.6.3  and  in  appendices  G  and  H. 
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In  the  following,  however,  two  different  methods  will  be  suggested 
in  order  to  improve  the  initial  values  of  veDocity  and  range.  The  approaches 
will  depend  on  whether  the  observer  is  maneuvering  or  not  over  the  time 
period  [0,  NT],  which  we  will  call  the  initialization  period. 

We  assume  that  the  following  data  are  available: 


1.  A  sequence  of  bearing  observations,  Z„  =  {(j)_,  (j),...*} 

N  0  X  N 


2.  The  observers  position  increments.  Ax  =  {Ax  .,Ax  ,...Ax  } 

S  sO'  si  St 

N-1  n-1 

and  Ay  =  {Ay  ,  Ay  , . . . ,  Ay  } . 

%-l  ®N-1 


Case  1,  Observer  Maneuver jjig 

Step  1:  Calculate:  R^=  f((()^,  'J’i+3^ '  i=0,l,. 

fxinction  f  is  defined  by  equation  (4.49) . 


. ,N-3.  The 


Step  2:  Calculate:  v^^  =  *^1'  *^2+1'  *^1+2^'  i«0,l,. ..  ,N-3. 

^yi  “  ^2^^i'  ^i'  '*’i+l'  ‘^i+2^  i=0,l, . . .  ,N-3. 

where  the  ftmctions  f^^  and  are  defined  by  equations  (4.25) 
and  (4.24) . 


Step  3:  Calculate  the  initial  velocity  elements  as; 


'^xO 


VyO  = 


N-3 

E  V 


"-2  a 


N-3 


*^-2  -^0  yi 


(4.53) 


(4.54) 


Step  4;  Calculate  the  teurget  course  as 

-l/^xo^ 


C^  =  tan 


yo; 


(4.55) 
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Step  5;  Calculate  initial  range,  R^,  from  equation  (4.37) 


Step  6;  Initial  bearing  is  given  as  (|)q. 


Step  7;  Initial  position  elements  are  calculated  as: 


^0  =  ^sO  +  ^0  ^0 


(4.56) 


^0  =  ^sO  ^0 


(4.57) 


Step  8; 


_  —  T 

Starting  with  Xq  =  ^0  ^xO  '  process  the  bearings 

{(b, ,  (j)-, _ _  4}  through  the  Kalman  filter,  to  give  the 

12  N 

resulting  state  vector  at  t  =  t^^,  where  the  track  is  offically 
started. 


Case  2,  Observer  not  maneuvering 

In  this  case,  equation  (4.49)  is  worthless,  and  no  range  information 
can  be  svibtracted  directly  from  the  bearings.  In  this  case,  the 
following  steps  are  proposed; 


Step  1;  Select  as  the  sensor's  maximum  range.  (If  possible, 
take  environmental  conditions  into  account) . 


Step  2;  Calculate:  v^^  =  ‘^i+1'  ‘^i+2^ 


V  °  h'  ♦i+2> 


"i+l  '  ‘s'"!'  v  *i'  Vl’ 


i  =  0,1,2, ...  ,N-3.  The  functions  f^^  and  f^  are  given  by 
equations  (4.25)  and  (4.24),  while  the  function  f^  can  be 
found  by  an  extension  of  equation  (4.16),  as; 


I 
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We  assume  that  the  same  data  are  available  as  in  Section  4.5.1, 
and  we  will  have  the  same  two  cases,  depending  on  the  observer's  maneuvering 
scheme ; 


Case  1,  Observer  Maneuvering. 

Steps  1-5;  Same  as  in  Section  4.5.1,  case  1. 

Step  6;  With  a  previously  defined  AR^^,  calculate  i=l,2,...,M,  as: 

"oi  =  »0  - 1  • 


Step  7;  Calculate  the  velocity  con^nents  v  and  v  from  equations 

Oi  ^Oi 

(4.25)  and  (4.24),  i=l,2,...,M. 


Step  8;  Initial  beeiring  for  each  of  the  M  filters  is  given  as  (J)^^  =  (J)^, 
i=l ,2 , . . . ,M. 


Step  9;  Initial  position  data  are  calculated  as: 

^Oi  =  ^sO  ^0 

^Oi  =  ^sO  ^Oi 
i=l,2 , ... ,M. 

Step  10:  Same  as  Section  4.5.1,  case  1,  step  8  for  each  filter. 

Case  2,  Observer  not  maneuvering 

Steps  1-6:  Same  steps  as  in  section  4.5.1,  case  2,  resulting  in  Rq  at 


the  end  of  step  6. 
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Step  7;  With  a  previously  defined  Ar^  >  AR, ,  calculate  R«.»  i«l,2,...,M, 

M  X  vX 

as: 


Oi 


~  i*AR 


2 


(4.66) 


(R^  is  assumed  to  be  neeir  the  maxixnura  range  for  the  becuring  sensor, 
and  any  R^^  >  R^  should  not  be  necessary) . 


Steps  8-11:  Same  steps  as  Case  1  above,  steps  7-10. 


4.6  Covcuriance  Matrix  Initialization 

The  next  svibject  to  be  addressed,  is  the  selection/calculation  of 
initial  values  for  the  covariance  matrix,  P^.  We  wi.ll  only  derive  the 
initial  covariance  matrix  for  the  Cartesicm  system  model.  (NecesscU^ 
transformation  to  the  Polar  coordinate  system  case  can  be  done  through 
equation  (3.48)). 

Since  the  philosophy  behind  the  parallel  filter  approach  is  totally 
different  from  the  single  Kalman  filter  approach  with  respect  to  assumed 
initial  accuracy  in  range,  these  two  cases  have  to  be  treated  separately 
also  under  this  heading. 

4.6.1  Single  Kalman  Filter  Ceise 

Three  different  approaches  to  covariance  matrix  initialization  will 
be  outlined  under  this  heading. 


4. 6. 1.1  Aidala's  Approach 

Aidala  [11]  has  treated  this  subject  very  thoroughly,  however,  his 
model  of  the  teurget  motion  analysis  problem  has  no  process  noise. 
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Consequently ,  his  time  update  equation  for  the  covariance  matrix  has 
the  form; 


(4.67) 
T 


i.e.,  no  increase  in  vincertainty  with  time,  since  the  term  6(T)Vj^0(T) 
from  equation  (3.18)  is  lacking,  since  his  conclusions  depends  entirely 
on  this  tinrealistic  assumption,  his  results  are  not  applicable  directly. 
Aidala  proposes  to  select 


^0  = 


(4.68) 


and  shows  this  selection's  superiority  in  terms  of  covariance  matrix 
stability  over  the  selection 


^0  = 


0 


V  J 


(4.69) 


By  a  pseudo linear  formulation  of  the  tracking  problem,  obtained 
through  a  nonlinear  transformation,  Aidala  transforms  the  nonlinear 
observation  equation  to  a  linear  equation,  with  the  nonlinearities 
embedded  in  the  observation  noise  (see  also  [7] .  The  measurement 
standard  deviation  for  this  model  turns  out  to  be; 


k 


(4.70) 
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cund  this  is  the  only  term  containing  R  and  in  the  covariance  eqpiations 
for  the  pseudo-linear  filter. 

The  covariance  matrix  of  the  pse\idolinear  filter,  is  related 

to  the  covariance  matrix  of  the  ori</inal  filter,  P.  .  r  in  the  following 

jC  /  JC 

way,  if  equation  (4.68)  is  used  for  initialization: 


By  selecting  =  R^,  equation  (4.70)  can  be  approximated  as: 

0  (4.72) 

K 

cuid  the  covariance  equations  for  •  the  pseudolinear  filter  is  completely 

independent  of  range  and  (P^  ^  =  I) . 

However,  the  reason  why  this  form  is  possible,  is  the  form  of  the 

covariance  time  update  equation  given  by  equation  (4.67).  If  equation 

2 

(3.18)  was  used,  the  initial  value  of  the  vcuciance,  Oq,  can  not  be 
collapsed  into  the  measurement  variance  term,  as  given  by  equation 
(4.70). 

Now,  is  it  feasible  to  adapt  Aidala's  results  for  our  model,  where 
time  vpdating  of  the  covariance  matrix  is  performed  according  to  equation 

(3.18) ? 

This  question  is  not  possible  to  answer  without  performing  simula¬ 
tions. 

For  the  case  with  a  nonmaneuvering  observer  during  the  initialization 
phase,  it  is  therefore  suggested  to  incorporate  the  selection  of  the 


initial  covariance  matrix 
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^0  =  «o  •  ^ 


(4.73) 


as  one  possibility  to  be  explored. 

4. 6. 1.2  General  Approach 

The  most  commonly  used  initial  covariance  matrix  is  probably  of  the 


form: 


P  K  p  = 

0,-1  0 


Pll  ^12 


^21  ^22 


0 

0 


^33  ^34 


0  ^43  ^44 


where : 


(4.74) 


Pll  «  cos\.(RQ.a^^)2  ^  sin\al 


(4.75) 


Pl2  =  P2I  = 


(4.76) 


(4.77) 


P33  =  cos^Cg.(vQ.ac^)2  +  sin^c^.a^^ 


(4.78) 


P34  =  ^*43  = 


(4.79) 


P44  =  sin^CQ(vQa^^)^  +  cos^Cq-0^^ 


(4.80) 


The  usual  (ad  hoc)  selection  of  parameter  values  in  equations  (4.751 


(4.80)  is  the  following; 
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({)q  =  the  observed  bearing  at  time  t^ 

0.  =  the  sensor's  standard  deviation 

’*’0 

R-  =  the  sensor's  max  range, 
u 

^■D  “  T  sensors  max.  range) 

%  ^ 

'0  -  -♦0 

V*,  0  and  0  :  completely  picked  out  of  the  air.  As  an  exaj>i>le; 

*^0  "^0 

Vq  =  10  m/sec 

0  =  30“ 

°0 

0  »  20  m/sec. 

^0 

4. 6. 1.3  Suggested  Approach 

If  the  state  initialization  methods  given  in  section  4.3  and  4.4 
are  used,  a  better  initial  covariance  matrix  can  be  obtained. 

Like  in  the  state  vector  initialization  case,  the  approach  will 
depend  on  the  observers  maneuvering  scheme: 

Case  1,  Observer  Manewering. 

Step  1;  Range  can  now  be  calculated  from  equation  (4.49).  Calculate 

2 

the  initial  variance  for  range,  0  ,  through  the  approach  given 

^0 

in  Appendix  G. 

Step  2;  The  velocity  elements  can  be  calculated  from  equations  (4.24) 

and  (4.25).  Calculate  the  velocity  elements  of  the  covariance  matrix 
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through  the  approach  given  in  Appendix  H. 

When  the  "smoothing"  approach  given  in  section  4.5.1  is  used,  the 
resulting  initial  values  of  the  state  vector  should  be  better  than  the 
resulting  initial  covariance  matrix  from  this  approach  will  indicate. 
However,  since  we  are  interested  in  em  initially  "open"  filter  (a 
filter  that  responds  to  the  measurements) ,  we  suggests  not  to  take 
this  into  account.  If  the  actual  acoiiracy  obtained  through  the  methods 
given  in  section  4.5.1  turns  out  to  be  much  higher  than  the  resulting 
covariance  from  this  approach,  it  will, however^  be  worthwhile  to  take  the 
"smoothing"  effect  into  account  also  for  initial  covariance  calculations. 

4.6.2  Parallel  Filter  Case 

With  M  filters  running  in  parallel,  we  are  interested  in  each 
filter  being  as  "stiff"  as  possible  in  range,  since  it  is  likely  that 
one  of  the  filters  have  an  initial  range  close  to  the  correct  one,  as 
will  sTobsequently  become  apparent  through  an  observer  maneuvre  (see 
section  3 . 7  and  4.5.2). 

For  the  parallel  filter  approach,  the  only  difference  between  the 
two  cases;  observer  maneuvering/not  maneuvering,  is  the  size  Ar  between 
the  initial  values  of  range  for  each  filter  (see  section  4.5.2,  or 

The  following  approach  is  suggested: 

Step  1:  Select  = 


2  • 
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step  2;  Select  0.  =  the  sensors  standard  deviation. 

'^0 

Step  3;  Given  the  initial  reinge  i=l,2,-..,M,  calculate  the 

position  covariance  elements  for  each  filter  from  equations 
(4.75)-(4.77) . 


Step  4;  Calculate  the  velocity  elements  of  the  covctriance  matrix 
for  each  lilter  through  the  approach  given  in  Appendix  H. 


5.  MANEUVRE  DETECTION  AND  HANDLING 


Up  to  this  point  we  have  assumed  constant  course  and  speed  for  the 
target.  This  assumption  will  now  be  removed,  and  we  will  allow  our 
target  to  make  abrupt  changes  in  course  and/or  speed,  of  random  sizes, 
and  occuring  at  random  time  instauits. 

Several  different  approaches  to  the  maneuvering  target  tracking 
problem  have  been  proposed  in  the  literature.  We  will  in  the  folj-owing 
give  a  short  survey  cl  some  of  the  most  important  approaches. 

Jazwinski's  [25]  limited  memory  filtering  approach  is  probably 
the  simplest  approach.  By  preventing  the  covariance  matrix  elements 
from  decaying  below  certain  thresholds,  resulting  in  filter  gains  above 
certain  values,  the  target  state  vector  dependence  on  the  latest  ob¬ 
servations  are  increased.  However,  the  tracking  performance  of  this 
approach  diiring  nonmaneuvering  periods  of  the  target  will  decrease 
due  to  higher  dependence  on  the  observation  noise. 

The  natural  solution  to  this  problem  is  to  model  the  target  under 
the  nonmaneuvering  hypothesis,  and  in  addition  to  ir Produce  some  adaptive 
maneuvre  detection  scheme  which  can  step  in  and  give  the  filter  limited 
memory  for  a  short  period  of  time,  after  a  maneuvre  is  detected  [4], 

[20]. 

A  further  extension  to  this  approach  is  suggested  by  Willsky  and 
Jones  [26] ,  [27] .  They  suggest  the  possibility  of  simultaneously  with 
the  detection  of  an  abrupt  system  change,  to  estimate  the  size  of  the 
cltange  and  to  perform  a  state  variable  correction  directly,  in  addition 
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to  give  the  filter  limited  memory  tei($)orarily.  Similar  approaches  are 
outlined  in  [9]  and  [10] . 

Another  avenue  along  which  many  researchers  have  been  working,  is 
to  model  the  target  mcineuvres  as  a  semi -Markov  process,  whereby  N  possible 
acceleration  inputs  cire  selected  according  to  some  a  priori  probabili¬ 
ties.  In  its  pure  form,  this  approach  requires  an  infinitely  growing 

2 

bank  of  paralltj.  'liters,  N  initially,  N  for  the  second  measurements , 
etc.  Different  approaches  to  reduce  this  described  growth  are  proposed 
in  the  literature,  in  order  to  get  practical,  realizable  filters 
[14],  [10],  [21],  [22],  [28],  [291. 

Another  interesting  approach  was  proposed  by  Teimey  et  al.  [24] . 

Two  extended  Kalman  filters  are  operating  in  parallel,  one  with  a 
large  eurtificial  system  noise  covariance  term  added  to  give  the  filter 
limited  memory  and  thereby  allowing  it  to  track  fast  maneuvres,  and 
the  other  filter  with  small  artificial  noise,  making  this  filter  re¬ 
stricted  to  tracking  of  constant  course/speed  trajectories.  Maneuvre 
detection  is  performed  by  comparing  the  behaviour  of  the  two  filters. 

None  of  the  approaches  to  maneuvering  target  tracking  resumed 
above,  deal  with  a  tracking  scheme  based  on  bearing  only  information. 

Due  to  the  low  observability  of  the  system, maneuvre  detection  und^r 
these  circumstances  are  more  difficult.  Most  of  the  papers  above  claim 
for  high  observability  in  order  to  make  efficient  maneuvering  target 
trackers. 

Another  important  fact  to  realize  is  the  possibility  to  sepcirate 
the  maneuvering  target  tracking  problem  into  two  subproblems: 
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1.  The  maneuvre  detection  problem - 

2.  The  mcuieuvre  handling  problem  (i.e. ,  When  the  maneuvre 
is  detected,  what  actions  should  be  taken  to  allow  the 
tracker  to  adapt  to  the  new  target  course/speed) - 

This  separation  is  imbedded  in  the  approaches  given  in  [4]  ,  [9]  ,  [24]  , 

[26]  [27] ,  and  will  also  be  used  in  the  approach  proposed  in  this  report. 

The  lacking  observability  during  time-periods  when  the  observer 

is  normaneuvering ,  results,  as  we  have  seen  in  Chapter  4,  in  the 

range/velocity  ambiguity.  Unless  special  preventing  actions  are 

taken,  the  Kalman  filter’s  reaction  to  a  target  maneuvre  can  be  a  range 

jvm^)  as  well  as  a  course/velocity  jump.  The  obvious  preventing  action 

is  to  keep  the  range  variance  0_  low,  forcing  the  filter  to  adapt  course/ 

R 

velocity  as  a  maneuvre  detection  reaction,  and  leave  the  range  to  target 

unchanged.  This  seems,  however,  only  feasible  to  do  if  we  have  arrived 

at  a  stable  target  track  with  correct  range  prior  to  the  maneuvre.  If 

the  target  maneuvre  takes  place  while  the  tracker  still  is  in  the  initiali- 

ration  phase,  with  a  poor  linearization  trajectory  in  range,  our  wish 

is  to  keep  a  high  range  variance  in  the  Kalman  filter,  so  that  the 

filter  easily  can  arrive  at  correct  range  if/when  the  observer  performs 

a  maneuvre,  and  range  becomes  observable. 

2 

These  conflicting  preferences  on  o  are  among  the  reasons  why 

R 

maneuvering  target  tracking  is  harder  to  solve  for  the  bearing  only 
measurement  case  than  for  cases  with  con^lete  observability. 

One  possible  way  to  resolve  this  conflict,  is  to  make  use  of  our 
knowledge  of  the  observers  position  history  and  future  maneuvre  in¬ 
tentions.  If,  for  the  global  iterated  filter  case,  the  observer  has  not 
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perfozmed  maneuvres  during  the  iteration  interval,  and  generally  for  all 

the  filter  approaches,  if  the  observer  is  not  going  to  start  maneuvering 

2 

in  the  near  future,  it  is  no  point  in  keeping  a  high,  since  range  can't 

R 

be  observed  from  the  observations  in  any  case.  However,  we  will  get  a 
possible  conflict  if  the  obseirver  and  the  target  are  maneuvering  at 
the  same  time.  The  solution  to  this  problem  has  to  be  decided  upon 
through  simulations. 

A  proposed  approach  to  maneuvre  detection  and  handling  will  be 
given  in  the  following. 


5.1  Maneuvre  Detection 

The  most  pCTi'arful  and  best  theoretical  fundamented  approach  to 
maneuvre  detection  seems  to  be  the  generalized  likelihood  ratio  best 
described  in  [30] .  This  approach  has  been  used  by  Willski'  and  Jones 
[26],  [27],  by  Tenney  et  al.  [4],  and  is  also  suggested  by  Maybeck 
[23]. 

Following  the  approach  given  in  these  references,  two  hypothesis 
on  che  form  of  the  innovation  signal  can  be  assumed: 


S  '’ik 


(No  maneuvre) 


maneuvre  has  occurred.) 


where  is  a  zero  mean  white  sequence  with  variance: 


%ij^  ”  ”k*\,k-l\  *  \ 


(5.1) 


If  we  restrict  our  attention  to  a  "data  window"  containing  the  N 
most  recent  observations,  our  generalized  likelihood  ratio  test  can  be 


-75“ 


given  by ; 


JV  C.  . 

-f  .1^ 


i=k-L  Hj 


where  c^,  L  and  y  cire  design  values  which  has  to  be  decided  upon  through 
simulations,  c^^  is  a  (possible)  varying  term  independent  of  the  observed 
residvial  values.  If  we  define: 


^Ik 


the  likelihood  ratio  test  can  be  given  by 


JL 


i=k-L  V, .  H 
ll  0 


(5.4) 


The  reasons  why  we  incorporate  the  possibility  of  having  a  varying 
threshold  are  the  following: 


1.  We  want  to  inhibit  maneuvre  detection  during  initialization 
phase . 

2.  We  want  to  inhibit  successive  maneuvre  detections  during  the 
same  maneuvre. 

3.  We  want  to  inhibit  loaneuvre  detections  during  maneuvering 
phases  of  the  observer,  if  we  have  reason  to  believe  that  our  lineari¬ 
zation  trajectory  prior  to  the  observer  maneuvre  is  poor. 


The  moaning  of  equation  (5.4)  is  obvious:  The  actual  variance  of  the 
innovation  signal  is  compared  with  its  expected  value  over  the  most 
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recent  L  sauries.  If  becomes  consistently  laxger  than  predicted  over 
the  selected  "time  window",  fx  tcirget  maneuvre  is  detected. 

A  few  guidelines  can  be  given  concerning  the  selection  of  the  design 
parameters  L,  Cj^  and  y  (alternatively  L  and  : 

L  has  to  be  selected  as  a  compromise  between  fast  detection  and  the 
false  detection  rate.  The  longer  time  window  the  slower  maneuvre  detection 
and  the  less  probc^ility  for  false  maneuvre  detection.  The  shorter  time 
window,  the  faster  maneuvre  detection,  but  at  the  same  time,  the  higher 
probability  for  false  detections. 

Y  can  be  decided  iipon  by  looking  at;  the  case  with  stationary 
circoastances :  Tracking  of  a  target  going  with  constaiit  course  and 
speed,  where  our  linearization  trajectory  is  approximately  correct. 

With  Cj^  »  0  and  the  process  noise  covariance  matrix  the  observation 
noise  variance  W  and  the  "time  window"  (decided  by  L)  given,  Y  can  be 
selected  to  give  a  false  aleirm  probability  close  to  zero. 

Cj^  has  to  depend  on  a  number  of  parameters,  and  may  have  different 
size  and  time  dependence,  depending  on  the  state  of  the  filter.  We 
may  suggest  the  following  structure  of  Cj^: 

I  -a  (k-k- )  T 

K  e  ^  k  >  k 

(5.5) 

0 

where  T  is  the  san^le  time,  and  k^^  is  the  san^le  when  a  certain  event 
takes  place,  like; 

1.  Initialization 

2 .  Maneuvre  detection 

3.  Starting  of  an  observer  mcineuvre 


The  parameters  K  and  a  are  event-dependent  parameters  which  has  to  be 
selected  through  simulations  in  order  to  give  acceptable  false  alarm 
probability  in  the  transient-period  following  the  actual  event.  These 
parameters  will  also  depend  on  the  "time  window"  LT,  on  V  ,  w  ,  P  and 

Jv  iC  U 

on  possible  direct  manipulation  on  the  covariance  matrix  following  a 
detected  manevtvre. 

The  maneuvre  detection  approach  described  in  this  section  should  be 
suitable  for  all  the  filtering  approaches  given  in  Chapter  3.  Whether 
it  is  the  best  approach  to  maneuvre  detection,  independent  of  the  filtering 
approach,  is  unknown. 

For  the  iterated  filtering  approaches  given  in  Chapter  3.4  to  3.6, 
we  would  suggest  to  include  an  the  simulation  study  an  investigation  on 
which  value  of  the  innovation  signal  that  should  be  used  in  equation  (5.4) , 
the  first  or  the  last  iteration  value. 

For  the  parallel  filter  case,  we  propose  to  run  the  maneuvre  detection 
algorithm  only  on  one  filter,  namely  the  filter  which,  at  each  time  in- 


stcint  kT ,  has  the  highest  probability  function  p 


5 . 2  Actions  Following  a  Detected  Maneuvre 

As  described  in  the  beginning  of  this  Chapter,  the  most  appropriate 
action  to  take  when  a  maneuvre  is  detected,  is  to  give  the  filtering 
algorithm  limited  memory  temporarily,  allowing  the  filter  to  adapt  to  the 
new  target  course  and  speed  more  easily. 

One  important  fact  to  realize,  hc«-’ever,  is  that  there  Is  a  certain 
time  delay  between  the  beginning  of  a  maneuvre  and  its  detection.  This 
time  delay,  say  At,  depends  on  a  number  of  factors,  however,  some  nominal 
value  of  At  should  be  possible  to  obtain  through  simulations. 
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If  we  don't  take  this  tinte  delay  into  account,  and  impose  limited 
memory  on  the  filtering  algorithm  only  from  the  time  instant  kT,  when  the 
raaneuvre  is  detected,  the  observations  taken  during  the  intermediate 
period  from  the  occtirrence  of  the  maneuvre  and  its  detection,  will  not  be 
utilized  optimally.  The  result  will  be  an  accumulated  range  error,  that 
can  not  be  driven  to  zero  unless  the  observer  performs  a  maneuvre  after 
the  tcirget  maneuvrt:. 

In  order  to  reduce  this  range  error,  even  for  the  case  of  a  non¬ 
maneuvering  observer  svibsequent  to  a  target  maneuvre,  the  following  steps 
are  suggested: 

1.  When  a  maneuvre  is  detected,  fetch  a  stored  version  of  the 
state  vector  and  the  covauriance  matrix  valid  at  time 
(kr-At)  from  the  conputer  memory. 

2.  Give  this  time  version  of  the  filter  limited  memory  by  increasing 
the  velocity  elements  of  the  covariance  matrix. 

3.  Re-integrate  all  the  observations  taken  in  the  time  interval 
[kr-At,  kT] . 

The  result  of  this  iteration  will  be  a  discrete  jump  to  a  more 
correct  position  and  velocity  for  the  state  vector  at  time  kT,  the  same 
time  instant  when  the  target  maneuvre  was  detected.  The  effect  of  this 
approach  is  thus  the  same  as  achieved  by  Willsky  and  Jones  [26] ,  [27] , 
and  estimate  of  the  size  of  system  change,  emd  a  direct  correction  of  the 
state  vector.  The  methods  are,  however,  different. 

The  prize  that  has  to  be  paid  for  this  achievement,  however,  is 
substantial.  Tlie  following  time  sequences  has  to  be  stored  in  the  computer 


memory: 
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Poleu:  coordinate  system  case)  through  the  following  equativons: 

2  2  2 
a  =  p  _*sin  c  -  (P- -+P,-)siT>r;  c  +  p .  .cos  c  (5.11) 

2  12  2 
0^  =  -j  IP33COS  c  +  °  (5.12) 

V 

Following  a  target  maneuvre  detection,  we  now  want  to  increase 
a  by  Ao  and  O  u,  ha  .  Then,  if  we  define: 

C  C  V  V 


6v  =  (a  +Aa  )^-a^  (5.13) 

V  V  V 

6c  =  (0  +Aa  )^  -  0^  (5.14) 

c  c  c 

the  result  on  the  velocity  elements  of  the  covariance  matrix  will  be; 


2  2  2 

P-.-.  P^o  +  V  cos  c  •  6c  +  sin  c  •  6v 

33  33 

2 

P-.4  +  sin  c  cos  c[<5v  -  v  6  c] 

34  34 

2 

P/ir>  P.(n  +  sin  C  cos  c[6v-v  6  c] 

43  43 

2  2  2 

P.*  P...I  +  V  -sin  c-6c  +  cos  c*6v 

44  44 


(5.15) 

(5.16) 


(5.17) 


(5.18) 


5.2.2  Actions  Dependent  on  the  Filtering  Approach 
The  design  parameters  in  the  maneuvre  detection  algorithm,  and  the 
actions  following  a  detection,  may  very  well  depend  on  the  tracking 
approach,  i.e.,  the  mathematical  models  of  the  system  dynamics  and  the 
observation,  and  tiie  version  of  the  filter  equations.  A  "tuning"  of  the 
maneuvre  detection  algorithm  to  each  filtering  approach  may  therefore 
be  necessary,  and  special  actions  following  a  maneuvre  detection  may  be 
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necessary  to  get  the  best  possible  performance  for  each  filtering  approach. 

In  the  following  we  will  discuss  some  of  the  actions  that  may  be 
necessary  (and  feasible)  to  take  for  the  global  iterated  filters,  and 
for  the  parallel  filter  approach.  Only  simulation  results  can  tell, 
however,  whether  further  "tuning"  of  parameters  or  special  actions  can 
give  better  performance  of  an  individvial  filtering  approach,  so  this  topic 
will  not  at  all  be  exhausted  by  the  following  discussion. 

5. 2. 2.1  Global  Iterated  Filters 

The  selection  of  the  process  noise  covariance  matrix  is  among 
the  parameters  that  influence  the  "covariance  level"  of  the  filter,  and 
thereby  the  size  of  the  elements  of  the  gain  matrix  In  fact,  the  degree 

of  limited  memory  can,  to  a  certain  extent,  be  controlled  by  Vj^. 

Generally,  is  decided  as  a  compromise  between  tracking  performance 
when  the  target  is  going  on  straight  course  and  speed,  and  the  filters 
ability  to  track  small  maneuvres  (without  alerting  the  maneuvre  detection 
cind  handling-system) . 

Now,  for  the  global  iterated  filters  described  in  sections  3.6.1  and 
3.6.2,  a  better  tracking  performance  can  be  obtained  during  nonmaneuvering 
periods  of  the  target  with  larger  values  of  the  elements  of  the  process 
noise  covariance  matrix  V^.  The  reason  for  this  is  the  smoothing  effect 
that  is  a  resut  of  the  iterations,  tending  to  stabilize  the  target  velocity 
vector  when  the  target  is  moving  with  constant  course  and  speed. 

At  the  same  time,  larger  values  on  the  elements  of  will  allow  the 
filter  to  follow  small  target  maneuvres  better. 

As  a  result  of  this,  however,  the  maneuvre  detection  problem  may 
faxn  ou\.  to  be  more  difficult,  since  higher  values  of  the  elements  of  the 


s' 

-82- 

gain-matrix  Kj^  will  tend  to  decrease  the  value  of  the  innovation  signal 
during  target  maneuvres,  at  the  same  time  as  the  expected  vcuriance  of 
the  innovation  signal  will  increase  due  to  higher  values  of  the  elements 
of  the  covariance  matrix  (see  equation  (5.1)).  An  obvious  result  will 
be  that  different  values  of  the  design-parameters  L,  y  and  c^^  have  to  be 
found . 

If  the  itei  ion  interval  [  (k-K/T,  kT]  ,  i^>rior  to  the  maneuvre 
detection ,  is  larger  than  the  interval  (kT-At ,  kT] ,  where  At  is  the 
nominal  time  delay  between  the  occurrence  of  a  maneuvre  and  its  detection, 
i.e.,  if  NT  >  At,  an  obvious  action  to  take  is  to  decrease  the  iteration 
interval  ten^xjrarily  in  such  a  way  that  it  is  contained  in  [kT-At,  kT] . 
(Premcineuvre  observations  should  not  be  taken  into  account  when  post- 
maneuvre  cotirse  and  speed  calculations  are  performed) .  since  the  ob¬ 
servations  taken  in  the  time  interval  I (kT-At) fkT]  comes  frcan  a 
maneuvering  target,  it  might  even  be  interesting  to  look  at  the  possi¬ 
bility  of  deviding  the  iteration  interval  [(k-N)T,  kT] ,  N  =  At/T,  into 
subintervals,  so  that  the  observations  contained  in  each  sub-interval 
are  more  consistent  with  the  constant  course  and  speed  hypothesis  on 
which  the  mathematical  model  of  the  system  dynamics  are  built.  A  reali¬ 
zation  of  this  idea  in  the  global  iteration  context  could  be  a  iteration 
interval  [(kj^-Nj^)T,  ,  where  N^^  <  N,  starting  with  =  k-N,  and 

finishing  when  k^^  =  k  (a  sliding  iteration  "window"  over  the  greater 
iteration  interval  [(k-N)T,  kT] ) . 

The  "serial"  filter  approach,  which  is  a  special  case  of  the  global 


iterated  filter  (see  section  3.6.3),  will  also  need  some  special  treatment 


First  r  inaneuvre  detection  slujuld  be  made  on 
innovation  signal  at  time  k,  (As  can  be  seen 

this  approach  has,  formally,  two  observations, 
consequently  two  innovation  signals,  and  ^  ) 


the  basis  of  the 
from  equation  (3.96) 
and  and 


3 


When  a  target  maneuvre  is  detected,  we  propose  to  reinitialize 
the  filter.  It  is  assmed  that  we  have  NT  >  At  (this  is  one  of  the 
design  criterieis  for  N) .  The  reinitalizing  sequence  will  consist  of 
the  following  steps: 

1.  Initialize  a  single  Cartesiam  Coordinate  System  filter 
with 


w 


2o  =  ^2k  “  4-N 

and 


(5.19) 


P  =  P 
0  22k, k 

where  .  is  the  lower  diagonal  part  of  the  co- 

A  A  Jv  f  JC 

variance  matrix  for  the  serial  filter,  given  by; 


(5.20) 


ilk,k 

^12k,k 

'21k, k 

^22k,k 

(5.21) 


Each  of  the  submatrices  in  equation  (5.21)  are  4  by  4 
matrices . 


2.  Run  this  single  filter  from  time  (k-N)T  up  to  time 
(kT-At) .  (Time  kT  is  the  time  when  the  maneuvre 
was  detected) . 


I 

I 
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3.  At  time  (kT-At) ,  in^ose  limited  memojry  on  this  single 
filter  as  described  in  section  5.2.1.  For  reasons  of 
convenience  of  notation,  we  now  define 


k^T  B  kT-At 


(5.22) 


4. 


Then  after  increasing  the  velocity  elements  of  the  covari2uice 

matrix  as  described  in  section  5.2.1,  the  resulting  value  of  the 

covariance  matrix  at  sample  k.  is  given  by  P  .  The  state 

*2 

vector  is  gxven  by  . 

“*2 

Run  the  single  filter  from  time  k^T  to  time  kT,  where  a 
discrete  jvnnp  in  the  position  and  velocity  at  time  kT  will 
be  the  result  (as  compared  to  x^j^  of  the  serial  filter  at 
time  of  manevtvrc  detection) . 


5.  Continue  to  run  the  single  filter  on  new  observations  to 
time  (k2+N)T.  Now  the  full  serial  filter  cein  be  initialized, 
with  the  following  initial  values: 


p 

k2-H« 

0 


(5.23) 


and 


(5.24) 


5. 2. 2. 2  Parallel  Filters 

As  was  proposed  in  section  3.7,  the  number  of  parallel  filters  can 
gradually  be  reduced  from  M  toweirds  1  as  the  filters  with  unlikely  ranges 
are  being  pruned  off.  It  is  therefore  obvious  that  we  have  to  consider 
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two  separate  cases  for  maneuvre  detection  with  this  approach,  namely: 


A.  Maneuvre  detection  when  the  filter  with  correct  range  is 
still  not  recognized. 

B.  Maneuvre  detection  when  only  the  filter  with  correct  range 
is  updated. 

In  order  to  save  computertime  in  Case  A,  we  propose  to  run  t^ic 

maneuvre  defection  algorithm  only  on  one  filter,  namely  rhe  filter  which, 

at  each  time  instant  kT,  has  the  highest  probability  function  p(Rj^  = 

i 

(Intuitively,  it  don't  seem  likely  that  one  of  the  parallel  filters  will 
be  better  in  identifying  a  maneuvre,  since  range  is  not  obser\fable.  If 
a  is  kept  low,  however,  amy  of  the  filters  should  be  able  to  detect  a 
target  course/speed  change) . 

For  the  parallel  filter  approach,  it  seems  likely  that  we  can  give 
up  our  claim  to  take  the  detection  delay  At  into  account,  without  degra¬ 
dation  in  tracJ'.ing  performance,  so  this  will  be  proposed.  The  reasons 
why  this  is  possible,  are  the  following; 


1.  After  maneuvre  detection,  independent  of  case  1  and  2  above, 
we  intend  to  reinitialize  all  the  M  filters,  with  different 
ranges  centered  around: 

Case  A:  The  range  calculated  from  the  state  vector 
given  by  equation  (3.110). 

Case  B:  The  range  given  by  the  remaining  filter. 

An  eventual  accumulated  range  error 
due  to  the  detection  delay  At,  will 
then  be  picked  up  by  one  of  the  re¬ 
initialized  filters. 
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In  case  A,  the  range  for  all  the  remaining  filters  may 
be  wrong  anyway,  even  at  the  time  when  the  maneuvre  started. 

If  the  time  delay  should  be  taken  into  account  in  case  A, 
we  would  have  to  store  the  state  vector  and  the  covciriance 
matrix  for  (in  the  worst  case)  all  the  M  filters  over  the 
last  N  samples,  where  NT  ^  At.  This  would  claim  for  a  lot 
of  comonter  memory  capacity. 


-87- 


eech  filter's  initial  range  can  be  calculated  from 
equation  (4.63) ,  (substitute  AR^  for  AR^^)  ,  and  the 
initial  position  data  subsequently  frcan  equations 
(4.64)  and  (4.65). 

Since,  in  this  Ccise,  the  range  difference  between 
each  filter  is  smaller,  the  remaining  filter's  velocity 
components  Ccui  be  used  as  initial  values  for  all  the 
M  filters. 

Concerning  the  re-initialization  values  for  the  M  covariance  matrices, 
we  propose  to  consider  two  possibilities: 


1.  Use  the  initialization  procedure  described  in  section  4.6.2. 


2. 


Use  the  covariance  matrix  for  the  remaining  filter  (case  B) , 

=  R/Zjj)  (case  A ) ,  but 
modify  its  elements  in  the  following  way; 


or  for  the  filter  with  highest 


■12, 


j  2.  2  ,„2  2. 

+  cos  4*0,  ‘(R.-R^) 

4>0  1  0 

-  sin  (j)Q  cos  <I>o-oJ^(rJ-Ro) 


P 


21. 

1 


0  0 


''22.  - 

where  R^,  p^^  ,  p^^  »  P21  »  ^22  ^0  referring 

to  the  center  filter,  and  R^,  p^^  ,  p^^  »  P2j^ 

P22  ,  i  =  1,2,...,M  are  the  initialization  values  of 
i 

the  position  elements  of  the  covariance  matrices  for 
the  M  parallel  filters. 

In  addition  to  this  ,  impose  limited  memory  on  the 
filters  by  increasing  the  velocity  elements  of  the  co- 
variance  matrices  as  described  in  section  5.2.1. 
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Which  of  the  two  proposed  methods  for  covarieuice  matrix  re¬ 
initialization  that  should  be  selected,  has  to  be  decided  v^n  throu^ 


simulations,  as  the  procedure  giving  the  best  result 


6.  SIMULATION  GUIDELINES 


The  development  of  filtering  approaches,  initialization  routines 
and  mcineuvering  tcirget  tracking  approaches  given  in  the  preceding  chapters, 
have  to  be  verified  and  conpared  tlarough  an  eactensive  simulation  study. 

Each  algorithm  should  be  optimized  with  respect  to  design  parameters 
like  process  noise  variance,  maneuvre  detection  parameters  and  actions,  etc., 
before  compcirison  between  the  different  approaches  can  be  performed. 

In  order  to  cirri ve  at  correct  coucltisions ,  the  target-obse:rver 
geometry  used  in  the  simulations  are  extremely  important.  On  the  other 
hand,  in  order  to  reduce  the  number  of  simulation  runs  to  an  acceptable 
level,  only  a  few  different  geometries  should  be  considered. 

The  simulation  study  should  further  focus  on  two  different  problems; 

1.  The  initialization  phase,  each  filters  ability  to  achieve 
correct  range  estimate  as  fast  as  possible. 

2.  Maneuvre  detection,  ability  to  track  a  target  through 
different  maneuvres  (small/large  course/speed  changes) . 

The  simulation  results  should  be  visualized  on  plots  giving  range 
error,  velocity  error  eind  course  error  as  a  functixxiof  time.  x,y-plots 
should  also  be  provided. 

It  is  believed  not  to  be  necessary  to  run  Monte  Carlo  simulation 
studies  on  all  the  10  different  approaches  given  in  chapter  3.  A  few 
single  simulation  runs  for  each  filter  approach  for  a  small  nvTnber  of 
target-observer  geometry  should  reveal  which  approaches  are  worth  further 
study. 

The  most  promising  2  or  3  approaches  should  then  finally  be  conpcured 
through  Monte  Carlo  simulations. 
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APPENDIX  A 


LINEARIZATION  OF  £  (^1_ 
We  have 


f  = 


.  -1  / 


xlt 


yk 


We  intend  to  calculate 


•(3^)  - 


3f 


3f 


_l 

34) 


3f,  3f 
3r“  ^ 


!h 

3v 


3f^  3f„  3f^  3f, 


^2 

34) 


‘2 

3R 


3v  3v 


(Al) 


(A2) 


In  order  to  be  able  to  calculate  the  different  elements  of  P (j^) ,  the 
following  derivatives  are  needed; 


^Nk 

34.k 


(A3) 


(A4) 
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OAL  . 

*  Nk 


(AS) 


3v 
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xk 


!5l 
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(A7) 


3v 


=  T  •  cos<(). 


(A8) 
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3Al 
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^xk 


-Tsin*^ 


(At) 


^Nk 

3—  =-Tsin4.j^ 
yk 


(AlO) 


3Al 


3v 


yk 


=  T  cos4). 


(All) 


Next,  we  define  the  following  two  variables: 
'^xk  *  <\  *  4l,p^)3in  +  iV  •  cox 

V  '  4l.p^)co3  -  41^^  sin 

Then  we  have: 


'^nk'\-^pk>  ^ 


\<V^pk) 

4l 


(A12) 

(A13) 


(A14) 
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1  -T[  (VAL^,^)sin  .t>^  ^  AI^  cos  -TAi.^ 


yk 


^+1 


4l 


af 


2  \ 


Vi 

af^  ^  T[{K^-^I^^^)sin^^  +  ia^cos 


av 


xk 


\+l 


\+l 


TAL 


3f^  Tl(R+^p^^)cos  -h  Al^sin  ^  _ik 


av 


yk 


\+i 


®k+l 


(A15) 


(A16) 


(A17) 


(A18) 


(A19) 


{A20) 


(A21) 


Equation  (A14)-(A21)  can  now  be  inserted  in  eq.  (A2) ,  and  we  have  our 
final  result; 


t 

\<V%k> 

-%k 

tAl  , 
yk 

-tAl  , 
xk 

2 

2 

2 

2 

\+l 

Vi 

\+l 

Vi 

t 

i 

m 

It 

\-%k 

Vffe 

■'V 

Wi 

® 

\+l 

®k+l 

*Sc+l 

\.i 

M 

0 

0 

1 

0 

S 

0 

0 

0 

1 

(A22) 


APPENDIX  B 


CALCULATION  OF  THE  ADDITIONAL  TERMS  a  AND  L.  FOR  THE  SECOND  ORDER 
GAUSSIAN  POLAR  COORDINATE  SYSTEM  MOD^  FILTER. 


1.  CALCULATION  OF  C, 
-k 


c  .  =  trace 
ki 


3 

’Sf.] 

1 

T 

*  k,k 

f  f  •  4 


(Bl) 


Now,  [3f^/3i^]  has  already  been  calculated  in  APPENDIX  A.  We  have: 


“i 

!f3 

!!« 


Vi 


-Nk 

tAl  , 
_ ^ 

-tAl  , 
xk 

4i 

\+l 

\+l 

\+l 

0 

1 

0 

0 

0 

1 

(B2) 


Next,  we  define: 


FI  = 


a 

W 

T 

3 

(B3) 
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^^24  ■■  T-  K  *  V 

®k+l 


*^2 '  '‘^41  •  \ 

Vi 


2T  AL  ,  AL  , 

fl  =  _ _ 

33  4 

®k+l 


^^34  =  ^  1 


®k+l 


^^41  “  \  *  ^^23 


^^42  “ 


7-  '’^+1  *k  -  2'^xk<W” 

\+l 


fl 


T^,«2 


43  “  4  ^®k+l  ■  ^xk^yk^ 

\+l 


^"44  =  -^S3 


^^11  "  ■  .,3 


'k+1 


[^pk\+l  ■*■  ’  ®k] 


'^X2  =  ^  K*1  -  %k  •  1 

Vi 


(B12) 


{B13) 


(B14) 


(B15) 


<B16) 


(B17) 


(B18) 


(Biy) 


(B20) 


(B21) 


(B22) 
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^^3  = 


\+l 


(B23) 


TR  2 

^^14* - 3  ^^yk^ 

Vl 


(B24) 


^+1 


(B25) 


al; 

f2  =  , 

22  -3 


Nk 


'k+1 


{B26) 


f2  =  — -  [4+1  sin  \  -  <V^pk^^^xk^ 
^k+1 


(B27) 


£2,4  -  \  '  ‘V'^pt’V’ 


<A 


(B28) 


^^31  ^^13 


(B29) 


^^32  ^^23 


(B30) 


£2 


33  =  -y-  -  ^4’ 

Vi 


(B31) 


T  AL  ,  -AL 


f2  - - 

34  p3 

\+l 


(B32) 


f2,,  =  f2 

41  14 


{B33) 


^^42  =  "^24 


(B34) 


^^43  =  ^^34 


{B35) 


f2  ^  = 
44 


(B36) 


Now,  having  calculated  all  the  elements  in  the  two  matrices  Fl  and  F2, 
the  vector  c^  cein  be  calculated  from  equation  (Bl) .  The  result; 

'Tv 


i,j=l  "3 


I  f2..P.. 
a  =  .  .  ,  31  13 

-k  1,3=1 


(B37) 


til 

where  P. .  is  the  (i,j)^  element  of  the  covariance  matrix  P  ,  . 
13 


2.  CALCULATION  OF 


The  matrix  Lj^  is  defined  by; 


1  2  ?  2 


(B38) 


In  order  to  be  able  to  calculate  we  first  define  te  two  matrices  S 


and  T: 


4  4  4 

Z  fl,  .P.,  Z  fl  .P.^ _ Z  fl  P.^ 

i=l  i=l  li  i4 


S  =  • 


Z  fl..P.,. 

3.=! 


E 

.  ,  4i  i4 
1=1 


{B39) 


Z  f2,.P. 

.  ,  li  il 

1  =  1 


Z  f2-  .P.  . 
.  ,  li  i4 
1  =  1 


Z  f2  .  .P_  , 
,  4i  il 
1=1 


.  T  4i  i4 
1  =  1 


{B40) 


Then  the  4  nonzero  elements  of  the  matrix  L^^  are  given  by: 


=  j  trace  (S-S) 


(B41) 


Si^2  ~  '2  (S.T  ) 


=  j  trace  (T.S) 


(B43) 


^22  “  J  trace  (T.T  ) 
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Finally,  the  matrix  Lj^  is  then  given  by; 


4  4 

■  £  s.  .s.  .  Z 

j,i=l  j,i=l 

4  4 

£  t  .3. .  Z  t..t. . 

j.U  "=>  j,i=l  "" 

\  "  2 


(B45) 


0  0 


APPENDIX  C 


ITERATED  EXTENDED  KAIMAN  FILTER.  DETAILED  EQUATIONS. 

The  Extended  Kalman  filter-equations  for  the  Cartesian  Coordinate 
system  representation  of  our  tracking  problem  are  given  by  equations 
(3.11)-(3.18) .  Equations  (3.13)  and  (3.14)  are  obtained  from  the 
following  equation  for  the  linearized  system: 


'  *4,k-i  ♦  \<*vV4,k-i> 

where 

*4,k '  4,k-4 

*4,k-i  =  'K’'>-®4-i,k-i 
*  v4,k-i 


(Cl) 


(C2) 

(03) 

(C4) 


(Cl) 


The  Extended  Kalman-filter  equation  (3.13)  is  developed 
through  the  special  selection  of  linearization  trajectory,  namely: 


^  =  ^,k-l 


(C6) 


When  the  observation  Zj^  is  processed,  ^  is  obtained,  and  the  system  is 
relinearized  about  ^  Then,  after  processing  of  and  relinearization, 
6xj^  ]c  ~  also  6x^  =  0  in  view  of  equation  (C3) .  As  we  can  see, 

equation  (Cl)  reduces  to  equations  (3.13)  and  (3.14). 

However,  the  Extended  Kalmanfilter  does  not  utilize  the  improved 
linearization  trajectory  ^  ^  for  the  processing  of  the  observation  z^. 
This  is  done  by  the  iteration  scheme  described  lay  Pig.  Cl.  The  £-vector 
has  to  be  decided  upon  through  simulations. 
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APPENDIX  D 


m 


m 


ITERATED  LINEAR  FILTER-SMOOTHER.  DETAILED  EQUATIONS. 

This  iteration  scheme  is  adapted  to  the  Polar  Coordinate  system 
representation  of  our  tracking  problem,  where  the  observation  equation  is 
linear.  That  means  some  sin®)lifications  in^osed  on  the  iterator  proposed 
by  Jazwinski  [2] . 

In  order  to  avoid  the  matrix  invertion  necessary  in  Jazwinski 's 
approach,  the  equation 


A 


'ii.r2k.i,k' 


(DM 


can  be  transformed,  making  it  unnecessary  to  calculate  S(k,  e^)  explicitly. 
We  have: 


=  'k.k'’'"'  "i>4l,k 


A 


-i+1  ^+l,k  ^i^  ^\+r\+l,k^ 


where 


K(k+1,  e.)  - 


Now,  if  we  define; 


{D2) 

(D3) 


(D4) 


L(k.i,  e.)  = 


(DS) 


which  gives; 


K(k+1,  e.)  =  £.) 


(D6) 
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-ice- 


equation  (Dl)  can  be  written,  inserting  for  and  s(k,  e^) ; 


iiti  =  4,k  ^ 


(D7) 


The  resulting  iterator  is  suimiarized  on  Fig.  Dl. 


STOP 


YES 


FIG.  Dl.  ITERATED  LINEAR 
FILTER"SMOOTHER 


APPENDIX  E 


DETAILED  DERIVATION  OP  THE  COVARIANCE  EQUATIONS 
FOR  THE  CARTESIAN  1CAU4AN  FILTER. 


In  this  Appendix  we  intend  to  derive  the  scalar  equations  for  the  ^ 

and  the  a  posteriori  covariance  imbedded  in  the  matrix  equations 
(3.18)  and  (3.16). 

The  purpose  "s,  if  possible,  to  assimilate  a  deeper  understanding 
of  the  Kalman  filter  mechanism  generating  the  expected  variance  on  the 
innovation  signal. 

In  the  following,  we  intend  to  utilize  our  knowledge  of  symmetry 
in  the  covariance  matrix  to  reduce  the  number  of  scalar  equations  from 
16  to  10  equations. 

Under  these  circumstances  equation  (3.16)  reduces  from  the  Joseph 
fom  to  the  form: 


’^k,k  “  ^^’W^.k-l 


(El) 


where  Kj^  and  is  given  by; 


(E2) 


cos  4)j^  sin  <t)j^ 


0  0 


(E3) 


The  variance  of  the  innovation  signal  is  given  by; 


i '  *  «k 


(E4) 


By  inserting  for  from  equation  (E3)  and  utilize  the  syranetry  fact 

of  the  covaricince  matrix,  eqpjation  (E4)  can  be  transformed  to; 
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2  1  2  2 
Oj^  =  *1*  ^22®^"  ■  ^12  ■*■  ''k 


where  the  subscript  k,k-l  are  dropped  on  the  elements  of  the  covariance 
matrix,  and  on  R  and  (j).  The  same  is  done  in  the  following. 

Now,  equation  (E2)  can  be  written  as: 


"  ^k,k-l“k  ■  ^2 


Next  we  define: 


\  =  «  • 


2  .  2 


Then  the  elements  of  the  K,  -matrix  can  be  calculated  to  be; 


*1*  *  ^  ‘*11  '‘’•*12 


Kjj.  =  ^  ‘*12  '•’-*22 


Sk  *  ‘*13  '‘’-*23  '•” 


(ElO) 


''4k  -  ^‘*14  '=°=  '•’-*24  '>• 


(Ell) 


Now,  from  equation  (E6)  we  have: 


Vk,k-1  =  ''k 


(E12) 


since  the  covariance  matrix  is  symmetric. 

Then,  inserting  from  equation  (E12)  in  equation  (El)  we  ge’ 


’^k,k  \,k-l  ■  °k  *  *k  ■  *Sc 


(E13) 
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Based  on  equation  {E13)  and  equations  (E7)-(Ell)  we  get  the  following 
10  scalar  covariance  equations: 


_k,k  _  k,k-l  1  rr.  A  «  • 

^1  ='^11  -  r  ♦-''12  ♦' 

k 


(E14) 


^12^  =  ~  ^^^11  *^“^12  ‘^“’^22 


k,k  -k,k-.  1 


P  '  =  p 

13  *^13 


^  cos  4>-I’3^2  ^^13  ^  "  **23 

^  (E16) 


^  -  ^-**11  *^“**12  ^^14  *^“^24 


_k,k  „k,k-l  1  r-  A  ^  •  j.,2 

^2  '®22  -5-  tti2  *1 


22 


(E18) 


^2^  ‘  -^^^12  ■  ^22  ^^13  '*’"^23 


(E19) 


^24^  ^  ^24^~^  ■  ^^^12  ^~^22  ^^14  ***”^24 

k 


_k,k  „k,k-l  1  rr.  A.  -r.  •  A1‘ 

"33  ■  ^33  -  ST  "is  =“  *-'23  '>> 

k 


(E21) 


^34^  =  **34*^'^  “  ^  ^**13  ‘^’"^23  ^^14  ***“’^24 

k 


(E22) 


^k,k  „k,k-l  1  rr^  ^  .i.,2 

®44  =  ''44  '  57  '®14  ♦-^24  ♦> 

4 


(E23] 


In  equations  (E14)-(E23) ,  loost  of  the  variables  on  the  wight  hand  side 


of  the  equations  have  got  their  subscript  k,k-l  dropped. 
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Now,  the  only  place  the  range  R  enters  the  equations  (E14)-(E23),  is 
through  Nj^.  From  equation  (E7)  and  (E5)  Nj^  can  be  given  by: 


2  2  2 
N  =  P,  n  ‘^c>s  <}>  +  P  sin  <!>-P  „  sin  2<{)  +  R  V?, 

iC  iC 


(E24) 


Equivalently,  if  we  define: 


2  2  2 

sin  ^  +  P22  cos  (j)  +  P^2  24> 


(E25) 


equation  (E24)  can  be  written: 


\  -  i  * 


(E26) 


Equations  {E14)  through  (E23)  gives  the  a  posteriori  covariance  after 
an  observation  intergration,  based  on  the  a  priori  covariance  prior  to 
the  observation  integration. 

Timeupdating  of  the  covariance,  i.e.,  calculating  the  a  priori  co- 
variance  at  the  next  sample,  based  on  the  a  posteriori  variance  at  the 
current  saitple,  is  performed  through  equation  (3.18).  By  multiplying 
out  this  matrix  equation,  utilizing  the  symmetric  properties  of  the 
covariance  matrix,  we  get  the  following  10  time  updating  equations: 

+  2T  P^'^  +  P^'^  +  (t4/4).V^j^  {E27) 


pk+l,k 

12 


+ 


T(P 


k,k 

23 


+  P 


k,k. 

14  ’ 


+ 


T^P 


k,k 

34 


(E28) 


,13/2). (E29) 


(E30) 
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APPENDIX  F 

CALCULATION  OF  INITIALIZATION  VALDES  FOR  RANGE 
AND  VELOCITY.  NUMERICAL  EXAMPLE. 


In  order  to  demonstrate  the  use  of  the  results  from  section  4.3  and 
4.4,  the  following  exan^Jle  is  constructed: 


In  Fig.  FI  we  have: 
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R„  =  5000m 

V 

-  V  =0  m/sec 

0 

SXi 

V  =  20  m/sec 

X 

V 

®yo 

=  '^sy^^  =  10  m/sec 

0 

II 

> 

V 

=  -10  m/sec 

y 

SX2 

At  =  25  sec 

V 

=  0  ro/sec 

^^2 

Based  on  this  data,  we  can  calculate  »  i  =  0,1,2,  and  (P^, 

i  =  0,1, 2, 3.  We  i.ave; 


Ay  n  =  Ay  ,  =  250  m 
■•sO  •'si 


AXg2  =  “250  m 


♦o-"* 


1 '  *=^'^[5000-230 " 

if  500  1  ~ 

[5000-500  J 

if  1750  1  ~ 

[500-500  J 


<P 

(p^  =  tan 
((»  =  tan 


12.53® 


21/25" 


Equation  (4.51)  then  gives 


= 


500  sin{4»2-<t>2^)  [sin  <|)2*cos  (j)^] 


0  sin  sin  -3  sin{(f)^-(})Q)  sin((j)2“<|)2) 


s  5001.54  n 


Prom  equation  (4.25)  and  (4.24)  we  obtain: 


^  ^  sin  (<I>j^-<[|q)  sin 

^  50  '  %  \  sin((f^-(^^) 


-  sin  4 


)=  “• 


01  m/sec 


APPENDIX  G 


CALCULATION  OF  INITIAL  VARIANCE  FOR  RANGE,  0. 


If  range,  R^,  is  calculated  from  equation  (4.49),  the  initial  value 
2 

of  0^  can  be  calculated. 

% 

We  assume  that  the  different  bearing  observations  are  statistical 

2 

independent  and  uncorrelated,  with  variance  O^.  The  bearing  observations 
are  further  statistical  independent  and  tincorrelated  with  the  observers 
position  increments,  Ax^,  /  i=0,...,2.  The  different  position  in¬ 
crements  are  also  assumed  statistical  independent  and  uncorrelated,  with 

2  2  2 

variance  0“^  =  0  =  0  . 

xs  ys  s 

We  now  define; 


Kl'  ■  <S-'o’^s2' 

(Gl) 

‘''si’  - 

(G3) 

(G4) 

Now,  by  use  of  equations  (G1)-(G4),  equation  (4.49)  can  be  written: 

sin((j)2-4>j^)  [yl'Sin^)^-  xl»cos  (l)^]  +  sin [x2  cos  (|)j^-y2  sin(|)^] 


0  At,At2  sin{(j)^-(})Q)sin((|)2-4>2^)-At2  (t^-t^)  .  sin(())^-^Q)  .sinCtl)^-!}*^) 

(G5) 


As  we  can  see,  equation  (G5)  can  be  expressed  as; 
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-117- 


(G6) 


If  we  develope  the  Taylor  expansion  for  R  about  scaae  nominal  value 

0 

Rq»  given  by: 


(G7) 


Rq  “  *^1'  ^2'  *^3' 


and  neglecting  u  '•los  higher  than  first  order,  we  get; 


-0  =  *0  "  %  ^  ^  *  |3«3  " 


We  now  define; 


^  8f  ^  X- 


IGB) 


5Rq  =  Rq-Rq 


We  then  have; 


0^  .  E{6rJ} 


{G9) 


(610) 


By  making  use  of  equation  (G8) ,  (g9)  and  (GIO)  we  get; 

fchfe  )■•  te^  (%)■]■ -5  •  (s. v)^ 

ft  •■■)’•(%•>■)'  '■ 


Now,  naming  the  demoninator  in  equation  (G5)  D,  and  the  nominator  N, 
we  calculate  the  different  derivatives  of  the  f-function.  We  have; 
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- 2  *  cos((()2-<j)Q)sin{4i2-<i>j^)  +  At^  (t2-tQ)cos  (({>^^-41^) 


sin(<|>2-(|)2)  ] 


(G12) 


8f 

1 

N-  I 

9f 

-  -i| 

3<^2 

2 

D 

N- 

3f 

1 

9^3 

2 

D 

N-  (At^At2Sin{(i)2-({)Q)cos(^2“‘}’i)  At2  (t2-tQ)cos  (4)j^-(j>Q)  sin  ((j)2-<|»2) )  1 

{G13) 


N*  (At^  •  At^  sin(<j)2-4)Q)cos((})2-<j>j^)  +  At2{t2-tQ)sin(4>j^-(j)p)cos(4>2-<l>2))] 

(G14) 


M  (At^At^cos  sin  (4>2~'i’i)  sin  cos  ^ 

(G15) 


1  ■  /A  A  ^  A 

^  =  -  -  sin«J.2-(|>,).cos  <t). 


(G16) 


|fl=  ^sin{(i.2-(!)^)sin({.3 


=  i  sin((i)3-<{>2)cos<!>, 


(G18) 
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||j  =  -  isin«J.3-(j)2)sin 


Further  we  have; 


4- 

r/3xl  \2 

/3xl  \2 

“xl  ' 

At^  .  lA.; 

t  At'  +  ( 

(3si_f  + 

(M.  .  f 
l^Ay^J 

Q 

to 

1-' 

II 

At^lAt^  .  At^  .  (t^- 

73x2  \2 
ISAx  „  ) 

L'  sO  ' 

/  3x2  \2' 

\3Ax^l)J 

>^yso/ 

/3y2  \2 

\3Ay^i;  J 

Uax  „)' 


32' 


2  2 


,2,_2 


=  {t.-t  )^(At^+At^)a^ 
8  ^0  2  1s 


We  now  define; 

3  \2 


K1 


‘  ifo 


and 


•  a-  = 


3f  „ 


Then  equation  (Gil)  can  be  written 


(G19) 


(G20a) 


(G20b) 


{G21a) 


(G21b) 


(G22) 


(G23) 


(G24) 


y  (G25) 
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«0  T  s 

The  variable  K2  can  be  calculated  to  be: 

^2  =  ^  ^sin^(<l'2-'{>i)  (2At2  +  (t2-tQ)^  +  sin^  (4)2-(j)^)  (t^-t^)  ^  (At^+At^)  ] 

(G27) 

Further,  K1  can  be  e:^ressed  as: 

K1  =  -^  [N^K3  -  2N-D.K4  +  D^B]  (G28) 

D 

where  K3,  K4  and  KB  are  given  by: 

K3  =  2{At^At2[cos^(4»2-<i)Q)sin^(4>2-<^T)  +  sin^  ((!)2-(j>Q)cos^  ] 

+  AtjAt^At^  sin^ 

(G29) 


K4  =  At^At2{2(yl  sincfi^-xl  coscji^)  sin  {^^-(J)^)  cos^  + 

[yl  cosij)^  +  xl  sinci)^]  cos(4)2-(fiQ)sin^  ((j>2-(i)^)  -  [x2  cos(j>^-y2  sin<|)^] . 

cos((f)2-<j)2)sin((j)^+4)2-<|)Q-4>2)  +  [x2  sin(})^+y2  cosi})^]  sin  ((|)2-(j)2)cos  ((j)2-(J>j^) 

sin  (<|>3-4'q)  } 

+  ^^2  (t3-tQ)  {  [yl  sin^-xl  cos<J)^]cos  (4)2-i}>j^)sin(<|)^-Hj)2-(J»jj-(j)2) 


-[yl  coscl)^  +  xl  sintfi^]  sin((J)2-4>3^)sin(4»^-<|)Q)cos(<|)2-(j)2)} 


(G30) 
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APPENDIX  H 


INITIAL  VALUES  FOR  THE  VELOCITY  COMPONENTS 
OF  THE  COVARIANCE  MATRIX 


Front  section  4.3,  equations  (4.24)  and  (4.25),  we  have  the  following 
expressions  for  the  target  velocity  ccm^nents: 


^At^  sin((j>j^-<|)Q)sin(j), 

''x  “  t^-tg  **oUt^  sin 


-  ^  ^sl  ^ 


sinij)  At  At 

— ; — TT — T— .  jcos  <|>iC7Z“  Ax  --Ax  ]  -  sin(j)  [7:—  Ay  -Ay  ,] 
sin((|)2-<p^)  •  1  At^  sO  si  l‘-At^  ■‘sO  •‘si-' 


1  r  sin(<|)j^-(J)^j)cos(f^ 

!“^0  -  ^^1 


''y  “  t, 


-  cos<J.jj}  +  Ay^jj  +  Ay^^  + 


cos  (J), 
sin((()  • 


The  following  accuracies  are  assumed  defined: 


a  =  a  =  a 

sx  sy  s 


<pQ  <{>2  <f> 
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Now,  in  order  to  sin^lify  the  equatiions  to  be  developed,  equations 
(HI)  and  (112)  can  be  written: 


\  =  ^l^^^si'  ^^si*  ^0'  ‘*’0'  *1^  *^2^ 

=  ^2^^i'  ^^si'  ^0'  *^0'  ♦l*  ^2^ 
where  i=0,l. 

We  now  develops  the  first  order  Taylor  expansion  for  f^^  and 
about  some  nominal  values  for  Ay^^,  ^q'  *^0'  *^1'  *^2' 

named  Ax^^,  Ax^j^»  ^^sO'  '^^sl'  ®0'  *^0'  “^l  ^2* 

following  equations: 


3f,  3f,  3f^  3f^  3f, 

6v  =  -XT - £Ax  ^  +  -rr -  6Ax  ,  +  —  (SAy  _  +  r* —  6Ay  ,  +  -r—  (SR- 

X  3AXgQ  sO  3AXg^  si  SAy^^  ^sO  SAy^^  'si  3Rq  0 


3f  3f,  3f 


{H6) 


3f. 


3f, 


3f. 


3f, 


3f, 


6v  =  37;^ —  5Ax  -  +  3-; --  ■  6Ax  ,  +  37-^ —  6Ay  ^  +  37^ —  SAy  .  +  3^  SR* 
y  3AXgQ  sO  3AXg^  sj.  BAy^^  'sO  3Ay^^  'si  3Rq  0 


3f 


3f 


3f- 


3(f),  '^‘*’1  ■*■  3(f)„  ■'■^2 


S<P, 


(H7) 


where : 


6v  =  V  -  V  =  V  -  f,  (•) 

X  X  X  X  1 

(H8) 

6v  =  v  -  V  =  v  -f„(*) 
y  y  y  y  2 

(H9) 
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6Ax  _  =  Ax  -  —  Ax  „ 
sO  sO  sO 


(HIO) 


5Ax  ,  =  Ax  ,  -  Ax^, 
si  si  si 


««0  '  V®0 


fi+o  -  V*o 


The  velocity  components  of  the  covariance  matcix  are  now  given  by: 


(H12) 


(H13) 


(HIS) 


(HIS) 


(HIT) 


^33  ^34 


^43  ^44 


e{ (6v  )  }  e{5v  6v  } 
X  ^  y 


e{6v  6v  }  E  (^v  )  } 
y  X  y 


We  now  assnine  that  Ax^q»  ^^sC'  ^^sl'  *^0'  ^2 


statistical  independent.  Then  we  have 


-[(Sfer-iy 

(%)'■%.[(%)■•  (St 
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P44  =  e{ (6Vy)  } 


LUxJ  "Uax^^/  M3AyJ  M3AyJJ 


s 


(H20) 


^34  ^43 


=  p{5v  6v  }  =  Va -  • 


Sf«  3f,  3f^ 

0^1  2 


*  3Ax  -  ‘  3Ax  ,  3Ax  , 

sO  sO  si  si 


8f, 


3f2  ^3f, 


3f, 


^^^sO  ^^^si  ^^^sl 


] 


s 


3f 

3R, 


i  !!i„2  J«i  ^j2jjx  ^fi], 

0  ’  ^“0  \  2^0  3^^  34.^34.2  34.2  J 


(H21) 


The  different  derivatives  of  £  and  f  are  given  in  the  following; 


3f 

3Ax 


1  1  r  ^2  “^i  ^^2 1 


(H22) 


3f^  ^  r  sin  cos  I 

L  ■  sin«j)2-<f.J  J 


{H23) 


3f, 


3Ay 


sO 


ti-t 


sin  4)  At 

- ^  ^  A^' 


2-‘-o  sin«t)2-(})^ 


(H24) 


3f, 


1  ^2 


^^^sl  ~  ^2'S  sin«|)2-<|)^)  *  *^1 


(H25) 


3f, 


^  cos  cos  <j)j^  At^ 


®^s0  ^2'^0 


{H26) 
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^^2 

1 

cos  cos  (jlj^ 

sin  (<j)2-i{>j^) 

-  ^  [l 

cos  (1>2  *  sin  <j>^ 

^2"^0  L 

sin  (<|)2-(|>j^) 

9f2 

-  ^  [l 

cos  sin 

'2"'=o  L 

■*■  sin{(J)2-4)j^)  J 

1  [''‘2 

sin(<j)^-<j)Q)sin  (ji^ 

9Ro  • 

'=2''=oI-N 

sin  (4>2-<|>  j^) 

1  r'^'a 

sin  (4^^-(J>q)  cos  <j>2 

3^0  ■ 

^2"^0 

sin 

-  sin  4 


-  cos  (j) 


.1 

J 


(H27) 


(H28) 


(H29) 


{H30) 


(H31) 


^0  r  At^  cos  sin 


^<•’0  "  ^2-^0  ’  L 


sin  ((j>2-4»^) 


+  cos  (J» 


J 


{H32) 


8f, 


3<|). 


=-^_r 

to-tn  L 


At^  cos  (((>^-(j>Q)  cos  (|>2 


'2  “O  ‘-^^1 


sin 


-  sin  (f 


,] 


(H33) 


94^ 


.  [: 

'^2-^0  L 


At^  sin  (j)^  •  sin((J)„-(|iQ) 


sin  (j). 


'0  At, 


2  0 
At2 

ST  ^sO  -  ^Sl  h=  *2 


sin 


sin  (4>2"'I>i 


-  I 

-•{•J  ' 


Jcos  4.3  *2}] 


(H34) 


'^2 

94., 


f.  ^ 

V-o  L  o^'i 


cos  4>2*sin(<j>2''<l>o) 
2 

sin  (4'2"*J’i) 


cos  (j). 


2 

sin  (<l>3-<l>j^) 


•{ 


Ax  .  -  Ax  ,  cos  4*  + 
sO  si  ^2 


jcos  *2  Ay^(,  -  Ay  J  sin 


(H35) 


